diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 091110b5f5148..ee0cd2e75930f 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -4,7 +4,7 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_common/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners common/autoware_auto_geometry/** opensource@apex.ai @autowarefoundation/autoware-global-codeowners -common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_auto_tf2/** jit.ray.c@gmail.com @autowarefoundation/autoware-global-codeowners common/autoware_point_types/** taichi.higashide@tier4.jp @autowarefoundation/autoware-global-codeowners common/autoware_testing/** adam.dabrowski@robotec.ai @autowarefoundation/autoware-global-codeowners @@ -17,14 +17,14 @@ common/fake_test_node/** opensource@apex.ai @autowarefoundation/autoware-global- common/global_parameter_loader/** kenji.miyake@tier4.jp @autowarefoundation/autoware-global-codeowners common/goal_distance_calculator/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners common/grid_map_utils/** maxime.clement@tier4.jp @autowarefoundation/autoware-global-codeowners -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp @autowarefoundation/autoware-global-codeowners common/polar_grid/** yukihiro.saito@tier4.jp @autowarefoundation/autoware-global-codeowners -common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp @autowarefoundation/autoware-global-codeowners +common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners common/signal_processing/** ali.boyali@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners common/tensorrt_common/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp @autowarefoundation/autoware-global-codeowners common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp kenji.miyake@tier4.jp makoto.yabuta@tier4.jp @autowarefoundation/autoware-global-codeowners @@ -117,8 +117,8 @@ planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4 planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planner/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners -planning/freespace_planning_algorithms/** takamasa.horibe@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners +planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp @autowarefoundation/autoware-global-codeowners planning/mission_planner/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp @autowarefoundation/autoware-global-codeowners planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp @autowarefoundation/autoware-global-codeowners diff --git a/.github/workflows/deploy-docs.yaml b/.github/workflows/deploy-docs.yaml index 270d04e6403d2..5f2d09d414806 100644 --- a/.github/workflows/deploy-docs.yaml +++ b/.github/workflows/deploy-docs.yaml @@ -4,6 +4,7 @@ on: push: branches: - main + - galactic paths: - mkdocs.yaml - "**/*.md" diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml index 7fd1cc90fe97d..bda722c87ef09 100644 --- a/.github/workflows/pre-commit.yaml +++ b/.github/workflows/pre-commit.yaml @@ -5,7 +5,7 @@ on: jobs: pre-commit: - if: ${{ github.event.repository.private }} + if: ${{ github.event.repository.private }} # Use pre-commit.ci for public repositories runs-on: ubuntu-latest steps: - name: Generate token diff --git a/.pre-commit-config-optional.yaml b/.pre-commit-config-optional.yaml index a805f1201c414..e0019e10d5210 100644 --- a/.pre-commit-config-optional.yaml +++ b/.pre-commit-config-optional.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/tcort/markdown-link-check - rev: v3.10.0 + rev: v3.10.3 hooks: - id: markdown-link-check args: [--config=.markdown-link-check.json] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b69ba032fe35c..5e57d3b6c0998 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,10 +1,9 @@ ci: - autofix_commit_msg: "ci(pre-commit): autofix" - autoupdate_commit_msg: "ci(pre-commit): autoupdate" + autofix_commit_msg: "style(pre-commit): autofix" repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-json - id: check-merge-conflict @@ -19,23 +18,23 @@ repos: args: [--markdown-linebreak-ext=md] - repo: https://github.com/igorshubovych/markdownlint-cli - rev: v0.32.1 + rev: v0.33.0 hooks: - id: markdownlint args: [-c, .markdownlint.yaml, --fix] - repo: https://github.com/pre-commit/mirrors-prettier - rev: v2.7.1 + rev: v3.0.0-alpha.4 hooks: - id: prettier - repo: https://github.com/adrienverge/yamllint - rev: v1.27.1 + rev: v1.29.0 hooks: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.7.1 + rev: v0.8.0 hooks: - id: flake8-ros - id: prettier-xacro @@ -45,35 +44,35 @@ repos: - id: sort-package-xml - repo: https://github.com/shellcheck-py/shellcheck-py - rev: v0.8.0.4 + rev: v0.9.0.2 hooks: - id: shellcheck - repo: https://github.com/scop/pre-commit-shfmt - rev: v3.5.1-1 + rev: v3.6.0-1 hooks: - id: shfmt args: [-w, -s, -i=4] - repo: https://github.com/pycqa/isort - rev: 5.10.1 + rev: 5.12.0 hooks: - id: isort - repo: https://github.com/psf/black - rev: 22.6.0 + rev: 23.1.0 hooks: - id: black args: [--line-length=100] - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v15.0.7 hooks: - id: clang-format types_or: [c++, c, cuda] - repo: https://github.com/cpplint/cpplint - rev: 1.6.0 + rev: 1.6.1 hooks: - id: cpplint args: [--quiet] diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 404ef468a50b3..5d99b8a463711 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -84,6 +84,12 @@ get_shape_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \return Marker ptr. Id and header will have to be set by the caller @@ -120,7 +126,7 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::Sha get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color); + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( @@ -131,10 +137,18 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n); @@ -143,9 +157,13 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points); + std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point /// \param val Point32 to be converted diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 2a5c7e53f484c..acf4a389ec6e3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -14,6 +14,8 @@ #ifndef OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ #define OBJECT_DETECTION__OBJECT_POLYGON_DISPLAY_BASE_HPP_ +#include "rviz_common/properties/enum_property.hpp" + #include #include #include @@ -59,8 +61,7 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase explicit ObjectPolygonDisplayBase(const std::string & default_topic) : m_marker_common(this), - m_display_3d_property{ - "Display 3d polygon", true, "Enable/disable height visualization of the polygon", this}, + // m_display_type_property{"Polygon Type", "3d", "Type of the polygon to display object", this}, m_display_label_property{"Display Label", true, "Enable/disable label visualization", this}, m_display_uuid_property{"Display UUID", true, "Enable/disable uuid visualization", this}, m_display_pose_with_covariance_property{ @@ -79,6 +80,18 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase m_line_width_property{"Line Width", 0.03, "Line width of object-shape", this}, m_default_topic{default_topic} { + m_display_type_property = new rviz_common::properties::EnumProperty( + "Polygon Type", "3d", "Type of the polygon to display object.", this, SLOT(updatePalette())); + // Option values here must correspond to indices in palette_textures_ array in onInitialize() + // below. + m_display_type_property->addOption("3d", 0); + m_display_type_property->addOption("2d", 1); + m_display_type_property->addOption("Disable", 2); + m_simple_visualize_mode_property = new rviz_common::properties::EnumProperty( + "Visualization Type", "Normal", "Simplicity of the polygon to display object.", this, + SLOT(updatePalette())); + m_simple_visualize_mode_property->addOption("Normal", 0); + m_simple_visualize_mode_property->addOption("Simple", 1); // iterate over default values to create and initialize the properties. for (const auto & map_property_it : detail::kDefaultObjectPropertyValues) { const auto & class_property_values = map_property_it.second; @@ -150,14 +163,22 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase const ClassificationContainerT & labels, const double & line_width) const { const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); - - if (m_display_3d_property.getBool()) { + if (m_display_type_property->getOptionInt() == 0) { return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + } else if (m_display_type_property->getOptionInt() == 1) { + return detail::get_2d_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width); } else { return std::nullopt; } } + template + visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + /// \brief Convert given shape msg into a Marker to visualize label name /// \tparam ClassificationContainerT List type with ObjectClassificationMsg /// \param centroid Centroid position of the shape in Object.header.frame_id frame @@ -246,7 +267,9 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase if (m_display_predicted_paths_property.getBool()) { const std::string uuid_str = uuid_to_string(uuid); const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); - return detail::get_predicted_path_marker_ptr(shape, predicted_path, predicted_path_color); + return detail::get_predicted_path_marker_ptr( + shape, predicted_path, predicted_path_color, + m_simple_visualize_mode_property->getOptionInt() == 1); } else { return std::nullopt; } @@ -366,8 +389,10 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase std::list m_class_group_properties; // Map to store class labels and its corresponding properties PolygonPropertyMap m_polygon_properties; - // Property to enable/disable height visualization of the polygon - rviz_common::properties::BoolProperty m_display_3d_property; + // Property to choose type of visualization polygon + rviz_common::properties::EnumProperty * m_display_type_property; + // Property to choose simplicity of visualization polygon + rviz_common::properties::EnumProperty * m_simple_visualize_mode_property; // Property to enable/disable label visualization rviz_common::properties::BoolProperty m_display_label_property; // Property to enable/disable uuid visualization diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index aec99ef805f5f..4771589dc9038 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -7,6 +7,8 @@ Apex.AI, Inc. Satoshi Tanaka Taiki Tanaka + Takeshi Miura + Apache 2.0 ament_cmake diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 2ef88880987be..73a524e130776 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -72,7 +72,7 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const std_msgs::msg::ColorRGBA & predicted_path_color) + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) { auto marker_ptr = std::make_shared(); marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; @@ -84,7 +84,7 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( marker_ptr->color.a = std::max( static_cast(std::min(static_cast(predicted_path.confidence), 0.999)), 0.5); marker_ptr->scale.x = 0.03 * marker_ptr->color.a; - calc_path_line_list(predicted_path, marker_ptr->points); + calc_path_line_list(predicted_path, marker_ptr->points, is_simple); for (size_t k = 0; k < marker_ptr->points.size(); ++k) { marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; } @@ -276,6 +276,38 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_auto_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->ns = std::string("shape"); + + using autoware_auto_perception_msgs::msg::Shape; + if (shape_msg.type == Shape::BOUNDING_BOX) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::CYLINDER) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::POLYGON) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } else { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } + + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = line_width; + marker_ptr->color = color_rgba; + + return marker_ptr; +} + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -392,6 +424,49 @@ void calc_bounding_box_line_list( points.push_back(point); } +void calc_2d_bounding_box_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // down surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); +} + void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -435,6 +510,21 @@ void calc_cylinder_line_list( } } +void calc_2d_cylinder_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double radius = shape.dimensions.x * 0.5; + { + constexpr int n = 20; + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = -shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + } +} + void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n) @@ -521,10 +611,39 @@ void calc_polygon_line_list( } } +void calc_2d_polygon_bottom_line_list( + const autoware_auto_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + if (shape.footprint.points.size() < 2) { + RCLCPP_WARN( + rclcpp::get_logger("ObjectPolygonDisplayBase"), + "there are no enough footprint to visualize polygon"); + return; + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } +} + void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, - std::vector & points) + std::vector & points, const bool is_simple) { + const int circle_line_num = is_simple ? 5 : 10; + for (int i = 0; i < static_cast(paths.path.size()) - 1; ++i) { geometry_msgs::msg::Point point; point.x = paths.path.at(i).position.x; @@ -535,7 +654,9 @@ void calc_path_line_list( point.y = paths.path.at(i + 1).position.y; point.z = paths.path.at(i + 1).position.z; points.push_back(point); - calc_circle_line_list(point, 0.25, points, 10); + if (!is_simple || i % 2 == 0) { + calc_circle_line_list(point, 0.25, points, circle_line_num); + } } } diff --git a/common/autoware_testing/autoware_testing/smoke_test.py b/common/autoware_testing/autoware_testing/smoke_test.py index 8c361474034ad..2594ddf7955b1 100644 --- a/common/autoware_testing/autoware_testing/smoke_test.py +++ b/common/autoware_testing/autoware_testing/smoke_test.py @@ -52,7 +52,6 @@ def resolve_node(context, *args, **kwargs): @pytest.mark.launch_test def generate_test_description(): - arg_package = DeclareLaunchArgument( "arg_package", default_value=["default"], description="Package containing tested executable" ) diff --git a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp index dde3f10e9b554..c2ed9f0b4bc1b 100644 --- a/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp +++ b/common/bag_time_manager_rviz_plugin/src/bag_time_manager_panel.cpp @@ -21,6 +21,8 @@ #include #include +#include + namespace rviz_plugins { BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel(parent) @@ -66,12 +68,17 @@ BagTimeManagerPanel::BagTimeManagerPanel(QWidget * parent) : rviz_common::Panel( void BagTimeManagerPanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); - client_pause_ = - raw_node_->create_client("/rosbag2_player/pause", rmw_qos_profile_services_default); - client_resume_ = - raw_node_->create_client("/rosbag2_player/resume", rmw_qos_profile_services_default); - client_set_rate_ = - raw_node_->create_client("/rosbag2_player/set_rate", rmw_qos_profile_services_default); + +// APIs taking rclcpp::QoS objects are only available in ROS 2 Iron and higher +#if RCLCPP_VERSION_MAJOR >= 18 + const auto qos_default = rclcpp::ServicesQoS(); +#else + const auto qos_default = rmw_qos_profile_services_default; +#endif + + client_pause_ = raw_node_->create_client("/rosbag2_player/pause", qos_default); + client_resume_ = raw_node_->create_client("/rosbag2_player/resume", qos_default); + client_set_rate_ = raw_node_->create_client("/rosbag2_player/set_rate", qos_default); } void BagTimeManagerPanel::onPauseClicked() diff --git a/common/interpolation/include/interpolation/spline_interpolation.hpp b/common/interpolation/include/interpolation/spline_interpolation.hpp index 9640bcd6c5120..f5befefeae9da 100644 --- a/common/interpolation/include/interpolation/spline_interpolation.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation.hpp @@ -70,9 +70,11 @@ class SplineInterpolation { public: SplineInterpolation() = default; - - void calcSplineCoefficients( - const std::vector & base_keys, const std::vector & base_values); + SplineInterpolation( + const std::vector & base_keys, const std::vector & base_values) + { + calcSplineCoefficients(base_keys, base_values); + } //!< @brief get values of spline interpolation on designated sampling points. //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, @@ -86,9 +88,21 @@ class SplineInterpolation // return value will be dx/dt(t) vector std::vector getSplineInterpolatedDiffValues(const std::vector & query_keys) const; + //!< @brief get 2nd differential values of spline interpolation on designated sampling points. + //!< @details Assuming that query_keys are t vector for sampling, and interpolation is for x, + // meaning that spline interpolation was applied to x(t), + // return value will be d^2/dt^2(t) vector + std::vector getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const; + + size_t getSize() const { return base_keys_.size(); } + private: std::vector base_keys_; interpolation::MultiSplineCoef multi_spline_coef_; + + void calcSplineCoefficients( + const std::vector & base_keys, const std::vector & base_values); }; #endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_ diff --git a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp index d44d16d88dd04..567a1873aaa5c 100644 --- a/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp +++ b/common/interpolation/include/interpolation/spline_interpolation_points_2d.hpp @@ -37,20 +37,19 @@ std::vector splineYawFromPoints(const std::vector & points); // SplineInterpolationPoints2d spline; // // memorize pre-interpolation result internally // spline.calcSplineCoefficients(base_keys, base_values); -// const auto interpolation_result1 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result1 = spline.getSplineInterpolatedPoint( // base_keys, query_keys1); -// const auto interpolation_result2 = spline.getSplineInterpolatedPoints( +// const auto interpolation_result2 = spline.getSplineInterpolatedPoint( // base_keys, query_keys2); -// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaws( +// const auto yaw_interpolation_result = spline.getSplineInterpolatedYaw( // base_keys, query_keys1); // ``` class SplineInterpolationPoints2d { public: SplineInterpolationPoints2d() = default; - template - void calcSplineCoefficients(const std::vector & points) + explicit SplineInterpolationPoints2d(const std::vector & points) { std::vector points_inner; for (const auto & p : points) { @@ -63,9 +62,22 @@ class SplineInterpolationPoints2d // std::vector getSplineInterpolatedPoints(const double width); // std::vector getSplineInterpolatedPoses(const double width); + // pose (= getSplineInterpolatedPoint + getSplineInterpolatedYaw) + geometry_msgs::msg::Pose getSplineInterpolatedPose(const size_t idx, const double s) const; + + // point geometry_msgs::msg::Point getSplineInterpolatedPoint(const size_t idx, const double s) const; + + // yaw double getSplineInterpolatedYaw(const size_t idx, const double s) const; + std::vector getSplineInterpolatedYaws() const; + + // curvature + double getSplineInterpolatedCurvature(const size_t idx, const double s) const; + std::vector getSplineInterpolatedCurvatures() const; + size_t getSize() const { return base_s_vec_.size(); } + size_t getOffsetIndex(const size_t idx, const double offset) const; double getAccumulatedLength(const size_t idx) const; private: diff --git a/common/interpolation/include/interpolation/zero_order_hold.hpp b/common/interpolation/include/interpolation/zero_order_hold.hpp index 1142cb544c174..966128321b470 100644 --- a/common/interpolation/include/interpolation/zero_order_hold.hpp +++ b/common/interpolation/include/interpolation/zero_order_hold.hpp @@ -21,37 +21,61 @@ namespace interpolation { -template -std::vector zero_order_hold( - const std::vector & base_keys, const std::vector & base_values, - const std::vector & query_keys, const double overlap_threshold = 1e-3) +inline std::vector calc_closest_segment_indices( + const std::vector & base_keys, const std::vector & query_keys, + const double overlap_threshold = 1e-3) { // throw exception for invalid arguments const auto validated_query_keys = interpolation_utils::validateKeys(base_keys, query_keys); - interpolation_utils::validateKeysAndValues(base_keys, base_values); - std::vector query_values; + std::vector closest_segment_indices(validated_query_keys.size()); size_t closest_segment_idx = 0; for (size_t i = 0; i < validated_query_keys.size(); ++i) { // Check if query_key is closes to the terminal point of the base keys if (base_keys.back() - overlap_threshold < validated_query_keys.at(i)) { closest_segment_idx = base_keys.size() - 1; } else { - for (size_t j = closest_segment_idx; j < base_keys.size() - 1; ++j) { + for (size_t j = base_keys.size() - 1; j > closest_segment_idx; --j) { if ( - base_keys.at(j) - overlap_threshold < validated_query_keys.at(i) && - validated_query_keys.at(i) < base_keys.at(j + 1)) { + base_keys.at(j - 1) - overlap_threshold < validated_query_keys.at(i) && + validated_query_keys.at(i) < base_keys.at(j)) { // find closest segment in base keys - closest_segment_idx = j; + closest_segment_idx = j - 1; + break; } } } - query_values.push_back(base_values.at(closest_segment_idx)); + closest_segment_indices.at(i) = closest_segment_idx; + } + + return closest_segment_indices; +} + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & closest_segment_indices) +{ + // throw exception for invalid arguments + interpolation_utils::validateKeysAndValues(base_keys, base_values); + + std::vector query_values(closest_segment_indices.size()); + for (size_t i = 0; i < closest_segment_indices.size(); ++i) { + query_values.at(i) = base_values.at(closest_segment_indices.at(i)); } return query_values; } + +template +std::vector zero_order_hold( + const std::vector & base_keys, const std::vector & base_values, + const std::vector & query_keys, const double overlap_threshold = 1e-3) +{ + return zero_order_hold( + base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold)); +} } // namespace interpolation #endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_ diff --git a/common/interpolation/package.xml b/common/interpolation/package.xml index 665e26bab9a0a..5a39d818a3238 100644 --- a/common/interpolation/package.xml +++ b/common/interpolation/package.xml @@ -6,6 +6,7 @@ The spline interpolation package Fumiya Watanabe Takayuki Murooka + Yutaka Shimizu Apache License 2.0 ament_cmake_auto diff --git a/common/interpolation/src/spline_interpolation.cpp b/common/interpolation/src/spline_interpolation.cpp index bd92af1007b50..1275b47346d78 100644 --- a/common/interpolation/src/spline_interpolation.cpp +++ b/common/interpolation/src/spline_interpolation.cpp @@ -85,10 +85,8 @@ std::vector spline( const std::vector & base_keys, const std::vector & base_values, const std::vector & query_keys) { - SplineInterpolation interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation interpolator(base_keys, base_values); // interpolate base_keys at query_keys return interpolator.getSplineInterpolatedValues(query_keys); @@ -266,3 +264,26 @@ std::vector SplineInterpolation::getSplineInterpolatedDiffValues( return res; } + +std::vector SplineInterpolation::getSplineInterpolatedQuadDiffValues( + const std::vector & query_keys) const +{ + // throw exceptions for invalid arguments + const auto validated_query_keys = interpolation_utils::validateKeys(base_keys_, query_keys); + + const auto & a = multi_spline_coef_.a; + const auto & b = multi_spline_coef_.b; + + std::vector res; + size_t j = 0; + for (const auto & query_key : validated_query_keys) { + while (base_keys_.at(j + 1) < query_key) { + ++j; + } + + const double ds = query_key - base_keys_.at(j); + res.push_back(2.0 * b.at(j) + 6.0 * a.at(j) * ds); + } + + return res; +} diff --git a/common/interpolation/src/spline_interpolation_points_2d.cpp b/common/interpolation/src/spline_interpolation_points_2d.cpp index 71e2629044f11..82c3841c424c5 100644 --- a/common/interpolation/src/spline_interpolation_points_2d.cpp +++ b/common/interpolation/src/spline_interpolation_points_2d.cpp @@ -73,14 +73,14 @@ std::array, 3> slerp2dFromXY( const std::vector & base_keys, const std::vector & base_x_values, const std::vector & base_y_values, const std::vector & query_keys) { - SplineInterpolation interpolator_x, interpolator_y; - std::vector yaw_vec; - // calculate spline coefficients - interpolator_x.calcSplineCoefficients(base_keys, base_x_values); - interpolator_y.calcSplineCoefficients(base_keys, base_y_values); + SplineInterpolation interpolator_x(base_keys, base_x_values); + SplineInterpolation interpolator_y(base_keys, base_y_values); const auto diff_x = interpolator_x.getSplineInterpolatedDiffValues(query_keys); const auto diff_y = interpolator_y.getSplineInterpolatedDiffValues(query_keys); + + // calculate yaw + std::vector yaw_vec; for (size_t i = 0; i < diff_x.size(); i++) { double yaw = std::atan2(diff_y[i], diff_x[i]); yaw_vec.push_back(yaw); @@ -94,10 +94,8 @@ std::array, 3> slerp2dFromXY( template std::vector splineYawFromPoints(const std::vector & points) { - SplineInterpolationPoints2d interpolator; - // calculate spline coefficients - interpolator.calcSplineCoefficients(points); + SplineInterpolationPoints2d interpolator(points); // interpolate base_keys at query_keys std::vector yaw_vec; @@ -112,6 +110,16 @@ template std::vector splineYawFromPoints( } // namespace interpolation +geometry_msgs::msg::Pose SplineInterpolationPoints2d::getSplineInterpolatedPose( + const size_t idx, const double s) const +{ + geometry_msgs::msg::Pose pose; + pose.position = getSplineInterpolatedPoint(idx, s); + pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(getSplineInterpolatedYaw(idx, s)); + return pose; +} + geometry_msgs::msg::Point SplineInterpolationPoints2d::getSplineInterpolatedPoint( const size_t idx, const double s) const { @@ -142,18 +150,64 @@ double SplineInterpolationPoints2d::getSplineInterpolatedYaw(const size_t idx, c throw std::out_of_range("idx is out of range."); } - double whole_s = base_s_vec_.at(idx) + s; - if (whole_s < base_s_vec_.front()) { - whole_s = base_s_vec_.front(); + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); + const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); + + return std::atan2(diff_y, diff_x); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedYaws() const +{ + std::vector yaw_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double yaw = getSplineInterpolatedYaw(i, 0.0); + yaw_vec.push_back(yaw); } - if (whole_s > base_s_vec_.back()) { - whole_s = base_s_vec_.back(); + return yaw_vec; +} + +double SplineInterpolationPoints2d::getSplineInterpolatedCurvature( + const size_t idx, const double s) const +{ + if (base_s_vec_.size() <= idx) { + throw std::out_of_range("idx is out of range."); } + const double whole_s = + std::clamp(base_s_vec_.at(idx) + s, base_s_vec_.front(), base_s_vec_.back()); + const double diff_x = spline_x_.getSplineInterpolatedDiffValues({whole_s}).at(0); const double diff_y = spline_y_.getSplineInterpolatedDiffValues({whole_s}).at(0); - return std::atan2(diff_y, diff_x); + const double quad_diff_x = spline_x_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + const double quad_diff_y = spline_y_.getSplineInterpolatedQuadDiffValues({whole_s}).at(0); + + return (diff_x * quad_diff_y - quad_diff_x * diff_y) / + std::pow(std::pow(diff_x, 2) + std::pow(diff_y, 2), 1.5); +} + +std::vector SplineInterpolationPoints2d::getSplineInterpolatedCurvatures() const +{ + std::vector curvature_vec; + for (size_t i = 0; i < spline_x_.getSize(); ++i) { + const double curvature = getSplineInterpolatedCurvature(i, 0.0); + curvature_vec.push_back(curvature); + } + return curvature_vec; +} + +size_t SplineInterpolationPoints2d::getOffsetIndex(const size_t idx, const double offset) const +{ + const double whole_s = base_s_vec_.at(idx) + offset; + for (size_t s_idx = 0; s_idx < base_s_vec_.size(); ++s_idx) { + if (whole_s < base_s_vec_.at(s_idx)) { + return s_idx; + } + } + return base_s_vec_.size() - 1; } double SplineInterpolationPoints2d::getAccumulatedLength(const size_t idx) const @@ -174,6 +228,6 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner( const auto & base_y_vec = base.at(2); // calculate spline coefficients - spline_x_.calcSplineCoefficients(base_s_vec_, base_x_vec); - spline_y_.calcSplineCoefficients(base_s_vec_, base_y_vec); + spline_x_ = SplineInterpolation(base_s_vec_, base_x_vec); + spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec); } diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index c2bb8eb177ba1..c95fc902ade44 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -13,7 +13,6 @@ // limitations under the License. #include "interpolation/spline_interpolation.hpp" -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include @@ -221,203 +220,18 @@ TEST(spline_interpolation, splineByAkima) } } -TEST(spline_interpolation, splineYawFromPoints) -{ - using tier4_autoware_utils::createPoint; - - { // straight - std::vector points; - points.push_back(createPoint(0.0, 0.0, 0.0)); - points.push_back(createPoint(1.0, 1.5, 0.0)); - points.push_back(createPoint(2.0, 3.0, 0.0)); - points.push_back(createPoint(3.0, 4.5, 0.0)); - points.push_back(createPoint(4.0, 6.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // size of base_keys is 1 (infeasible to interpolate) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - - EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); - } - - { // straight: size of base_keys is 2 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - - const std::vector ans{0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } - - { // straight: size of base_keys is 3 (edge case in the implementation) - std::vector points; - points.push_back(createPoint(1.0, 0.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - const std::vector ans{0.9827937, 0.9827937, 0.9827937}; - - const auto yaws = interpolation::splineYawFromPoints(points); - for (size_t i = 0; i < yaws.size(); ++i) { - EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); - } - } -} - TEST(spline_interpolation, SplineInterpolation) { - SplineInterpolation s; - // curve: query_keys is random const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; const std::vector query_keys{0.0, 8.0, 18.0}; const std::vector ans{-0.075611, 0.997242, 1.573258}; - s.calcSplineCoefficients(base_keys, base_values); + SplineInterpolation s(base_keys, base_values); const std::vector query_values = s.getSplineInterpolatedValues(query_keys); for (size_t i = 0; i < query_values.size(); ++i) { EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); } } - -TEST(spline_interpolation, SplineInterpolationPoints2d) -{ - using tier4_autoware_utils::createPoint; - - SplineInterpolationPoints2d s; - - // curve - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - points.push_back(createPoint(5.0, 10.0, 0.0)); - points.push_back(createPoint(10.0, 12.5, 0.0)); - - s.calcSplineCoefficients(points); - - { // point - // front - const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); - EXPECT_NEAR(front_point.x, -2.0, epsilon); - EXPECT_NEAR(front_point.y, -10.0, epsilon); - - // back - const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); - EXPECT_NEAR(back_point.x, 10.0, epsilon); - EXPECT_NEAR(back_point.y, 12.5, epsilon); - - // random - const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); - EXPECT_NEAR(random_point.x, 5.3036484, epsilon); - EXPECT_NEAR(random_point.y, 10.3343074, epsilon); - - // out of range of total length - const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); - EXPECT_NEAR(front_out_point.x, -2.0, epsilon); - EXPECT_NEAR(front_out_point.y, -10.0, epsilon); - - const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); - EXPECT_NEAR(back_out_point.x, 10.0, epsilon); - EXPECT_NEAR(back_out_point.y, 12.5, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); - } - - { // yaw - // front - EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); - - // back - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); - - // random - EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); - - // out of range of total length - EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); - EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); - - // out of range of index - EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); - EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); - } - - { // accumulated distance - // front - EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); - - // back - EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); - - // random - EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); - - // out of range of index - EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); - EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); - } - - // size of base_keys is 1 (infeasible to interpolate) - std::vector single_points; - single_points.push_back(createPoint(1.0, 0.0, 0.0)); - EXPECT_THROW(s.calcSplineCoefficients(single_points), std::logic_error); -} - -TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - using tier4_autoware_utils::createPoint; - - std::vector points; - points.push_back(createPoint(-2.0, -10.0, 0.0)); - points.push_back(createPoint(2.0, 1.5, 0.0)); - points.push_back(createPoint(3.0, 3.0, 0.0)); - - std::vector trajectory_points; - for (const auto & p : points) { - TrajectoryPoint tp; - tp.pose.position = p; - trajectory_points.push_back(tp); - } - - SplineInterpolationPoints2d s_point; - s_point.calcSplineCoefficients(points); - s_point.getSplineInterpolatedPoint(0, 0.); - - SplineInterpolationPoints2d s_traj_point; - s_traj_point.calcSplineCoefficients(trajectory_points); - s_traj_point.getSplineInterpolatedPoint(0, 0.); -} diff --git a/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp new file mode 100644 index 0000000000000..a7d7012c19e22 --- /dev/null +++ b/common/interpolation/test/src/test_spline_interpolation_points_2d.cpp @@ -0,0 +1,222 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include +#include + +constexpr double epsilon = 1e-6; + +TEST(spline_interpolation, splineYawFromPoints) +{ + using tier4_autoware_utils::createPoint; + + { // straight + std::vector points; + points.push_back(createPoint(0.0, 0.0, 0.0)); + points.push_back(createPoint(1.0, 1.5, 0.0)); + points.push_back(createPoint(2.0, 3.0, 0.0)); + points.push_back(createPoint(3.0, 4.5, 0.0)); + points.push_back(createPoint(4.0, 6.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + const std::vector ans{1.3593746, 0.9813541, 1.0419655, 0.8935115, 0.2932783}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // size of base_keys is 1 (infeasible to interpolate) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + + EXPECT_THROW(interpolation::splineYawFromPoints(points), std::logic_error); + } + + { // straight: size of base_keys is 2 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + + const std::vector ans{0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } + + { // straight: size of base_keys is 3 (edge case in the implementation) + std::vector points; + points.push_back(createPoint(1.0, 0.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + const std::vector ans{0.9827937, 0.9827937, 0.9827937}; + + const auto yaws = interpolation::splineYawFromPoints(points); + for (size_t i = 0; i < yaws.size(); ++i) { + EXPECT_NEAR(yaws.at(i), ans.at(i), epsilon); + } + } +} + +TEST(spline_interpolation, SplineInterpolationPoints2d) +{ + using tier4_autoware_utils::createPoint; + + // curve + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + points.push_back(createPoint(5.0, 10.0, 0.0)); + points.push_back(createPoint(10.0, 12.5, 0.0)); + + SplineInterpolationPoints2d s(points); + + { // point + // front + const auto front_point = s.getSplineInterpolatedPoint(0, 0.0); + EXPECT_NEAR(front_point.x, -2.0, epsilon); + EXPECT_NEAR(front_point.y, -10.0, epsilon); + + // back + const auto back_point = s.getSplineInterpolatedPoint(4, 0.0); + EXPECT_NEAR(back_point.x, 10.0, epsilon); + EXPECT_NEAR(back_point.y, 12.5, epsilon); + + // random + const auto random_point = s.getSplineInterpolatedPoint(3, 0.5); + EXPECT_NEAR(random_point.x, 5.3036484, epsilon); + EXPECT_NEAR(random_point.y, 10.3343074, epsilon); + + // out of range of total length + const auto front_out_point = s.getSplineInterpolatedPoint(0.0, -0.1); + EXPECT_NEAR(front_out_point.x, -2.0, epsilon); + EXPECT_NEAR(front_out_point.y, -10.0, epsilon); + + const auto back_out_point = s.getSplineInterpolatedPoint(4.0, 0.1); + EXPECT_NEAR(back_out_point.x, 10.0, epsilon); + EXPECT_NEAR(back_out_point.y, 12.5, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedPoint(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedPoint(5, 0.0), std::out_of_range); + } + + { // yaw + // front + EXPECT_NEAR(s.getSplineInterpolatedYaw(0, 0.0), 1.3593746, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.0), 0.2932783, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedYaw(3, 0.5), 0.7757198, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedYaw(0.0, -0.1), 1.3593746, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedYaw(4, 0.1), 0.2932783, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedYaw(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedYaw(5, 0.0), std::out_of_range); + } + + { // curvature + // front + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0, 0.0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.0), 0.0, epsilon); + + // random + EXPECT_NEAR(s.getSplineInterpolatedCurvature(3, 0.5), -0.2441671, epsilon); + + // out of range of total length + EXPECT_NEAR(s.getSplineInterpolatedCurvature(0.0, -0.1), 0.0, epsilon); + EXPECT_NEAR(s.getSplineInterpolatedCurvature(4, 0.1), 0.0, epsilon); + + // out of range of index + EXPECT_THROW(s.getSplineInterpolatedCurvature(-1, 0.0), std::out_of_range); + EXPECT_THROW(s.getSplineInterpolatedCurvature(5, 0.0), std::out_of_range); + } + + { // accumulated distance + // front + EXPECT_NEAR(s.getAccumulatedLength(0), 0.0, epsilon); + + // back + EXPECT_NEAR(s.getAccumulatedLength(4), 26.8488511, epsilon); + + // random + EXPECT_NEAR(s.getAccumulatedLength(3), 21.2586811, epsilon); + + // out of range of index + EXPECT_THROW(s.getAccumulatedLength(-1), std::out_of_range); + EXPECT_THROW(s.getAccumulatedLength(5), std::out_of_range); + } + + // size of base_keys is 1 (infeasible to interpolate) + std::vector single_points; + single_points.push_back(createPoint(1.0, 0.0, 0.0)); + EXPECT_THROW(SplineInterpolationPoints2d{single_points}, std::logic_error); +} + +TEST(spline_interpolation, SplineInterpolationPoints2dPolymorphism) +{ + using autoware_auto_planning_msgs::msg::TrajectoryPoint; + using tier4_autoware_utils::createPoint; + + std::vector points; + points.push_back(createPoint(-2.0, -10.0, 0.0)); + points.push_back(createPoint(2.0, 1.5, 0.0)); + points.push_back(createPoint(3.0, 3.0, 0.0)); + + std::vector trajectory_points; + for (const auto & p : points) { + TrajectoryPoint tp; + tp.pose.position = p; + trajectory_points.push_back(tp); + } + + SplineInterpolationPoints2d s_point(points); + s_point.getSplineInterpolatedPoint(0, 0.); + + SplineInterpolationPoints2d s_traj_point(trajectory_points); + s_traj_point.getSplineInterpolatedPoint(0, 0.); +} diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index 6a987dc269e76..cfb945270ed7f 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -45,7 +45,7 @@ namespace motion_utils * @param input_path input path(point) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -53,7 +53,7 @@ namespace motion_utils */ std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -63,7 +63,7 @@ std::vector resamplePointVector( * based on the interpolated position x and y. * @param input_path input path(position) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -71,7 +71,7 @@ std::vector resamplePointVector( */ std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path(poses). Note that in a default setting, position xy are @@ -81,7 +81,7 @@ std::vector resamplePointVector( * @param input_path input path(poses) to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -89,7 +89,7 @@ std::vector resamplePointVector( */ std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** @@ -99,7 +99,7 @@ std::vector resamplePoseVector( * based on the interpolated position x and y. * @param input_path input path(poses) to resample * @param resample_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -107,7 +107,7 @@ std::vector resamplePoseVector( */ std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true); + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true); /** * @brief A resampling function for a path with lane id. Note that in a default setting, position xy @@ -119,7 +119,7 @@ std::vector resamplePoseVector( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -129,7 +129,7 @@ std::vector resamplePoseVector( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -141,7 +141,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * and is_final are also interpolated by zero order hold * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -152,7 +152,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); @@ -165,7 +165,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( * @param input_path input path to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -175,7 +175,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true); /** @@ -186,7 +186,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * forward difference method based on the interpolated position x and y. * @param input_path input path to resample * @param resampled_interval resampling interval point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -197,7 +197,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy = false, const bool use_lerp_for_z = true, + const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true); /** @@ -210,7 +210,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( * @param input_trajectory input trajectory to resample * @param resampled_arclength arclength that contains length of each resampling points from initial * point - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -220,7 +220,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy = false, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true); /** @@ -233,7 +233,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( * method based on the interpolated position x and y. * @param input_trajectory input trajectory to resample * @param resampled_interval resampling interval - * @param use_lerp_for_xy If true, it uses linear interpolation to resample position x and + * @param use_akima_spline_for_xy If true, it uses linear interpolation to resample position x and * y. Otherwise, it uses spline interpolation * @param use_lerp_for_z If true, it uses linear interpolation to resample position z. * Otherwise, it uses spline interpolation @@ -245,7 +245,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( */ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy = false, + const double resample_interval, const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_twist = true, const bool resample_input_trajectory_stop_point = true); } // namespace motion_utils diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 5417da86f5359..e9b6dcbc34cce 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -21,7 +21,7 @@ namespace motion_utils { std::vector resamplePointVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -60,9 +60,12 @@ std::vector resamplePointVector( const auto spline = [&](const auto & input) { return interpolation::spline(input_arclength, input, resampled_arclength); }; + const auto spline_by_akima = [&](const auto & input) { + return interpolation::splineByAkima(input_arclength, input, resampled_arclength); + }; - const auto interpolated_x = use_lerp_for_xy ? lerp(x) : spline(x); - const auto interpolated_y = use_lerp_for_xy ? lerp(y) : spline(y); + const auto interpolated_x = use_akima_spline_for_xy ? lerp(x) : spline_by_akima(x); + const auto interpolated_y = use_akima_spline_for_xy ? lerp(y) : spline_by_akima(y); const auto interpolated_z = use_lerp_for_z ? lerp(z) : spline(z); std::vector resampled_points; @@ -82,7 +85,7 @@ std::vector resamplePointVector( std::vector resamplePointVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -102,12 +105,12 @@ std::vector resamplePointVector( resampling_arclength.push_back(input_length); } - return resamplePointVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePointVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } std::vector resamplePoseVector( const std::vector & points, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { // validate arguments @@ -120,7 +123,7 @@ std::vector resamplePoseVector( position.at(i) = points.at(i).position; } const auto resampled_position = - resamplePointVector(position, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePointVector(position, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); std::vector resampled_points(resampled_position.size()); @@ -148,7 +151,7 @@ std::vector resamplePoseVector( std::vector resamplePoseVector( const std::vector & points, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z) { const double input_length = motion_utils::calcArcLength(points); @@ -168,12 +171,12 @@ std::vector resamplePoseVector( resampling_arclength.push_back(input_length); } - return resamplePoseVector(points, resampling_arclength, use_lerp_for_xy, use_lerp_for_z); + return resamplePoseVector(points, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z); } autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -240,12 +243,16 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + auto closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -279,7 +286,7 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v, const bool resample_input_path_stop_point) { // validate arguments @@ -339,12 +346,13 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, use_zero_order_hold_for_v); + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, + use_zero_order_hold_for_v); } autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_v) { // validate arguments @@ -385,12 +393,18 @@ autoware_auto_planning_msgs::msg::Path resamplePath( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_v) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_v ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_v ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -420,8 +434,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath( autoware_auto_planning_msgs::msg::Path resamplePath( const autoware_auto_planning_msgs::msg::Path & input_path, const double resample_interval, - const bool use_lerp_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, - const bool resample_input_path_stop_point) + const bool use_akima_spline_for_xy, const bool use_lerp_for_z, + const bool use_zero_order_hold_for_twist, const bool resample_input_path_stop_point) { // validate arguments if (!resample_utils::validate_arguments(input_path.points, resample_interval)) { @@ -473,13 +487,13 @@ autoware_auto_planning_msgs::msg::Path resamplePath( } return resamplePath( - input_path, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_path, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); } autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const std::vector & resampled_arclength, const bool use_lerp_for_xy, + const std::vector & resampled_arclength, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist) { // validate arguments @@ -537,12 +551,18 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const auto lerp = [&](const auto & input) { return interpolation::lerp(input_arclength, input, resampled_arclength); }; + + std::vector closest_segment_indices; + if (use_zero_order_hold_for_twist) { + closest_segment_indices = + interpolation::calc_closest_segment_indices(input_arclength, resampled_arclength); + } const auto zoh = [&](const auto & input) { - return interpolation::zero_order_hold(input_arclength, input, resampled_arclength); + return interpolation::zero_order_hold(input_arclength, input, closest_segment_indices); }; const auto interpolated_pose = - resamplePoseVector(input_pose, resampled_arclength, use_lerp_for_xy, use_lerp_for_z); + resamplePoseVector(input_pose, resampled_arclength, use_akima_spline_for_xy, use_lerp_for_z); const auto interpolated_v_lon = use_zero_order_hold_for_twist ? zoh(v_lon) : lerp(v_lon); const auto interpolated_v_lat = use_zero_order_hold_for_twist ? zoh(v_lat) : lerp(v_lat); const auto interpolated_heading_rate = lerp(heading_rate); @@ -579,7 +599,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( const autoware_auto_planning_msgs::msg::Trajectory & input_trajectory, - const double resample_interval, const bool use_lerp_for_xy, const bool use_lerp_for_z, + const double resample_interval, const bool use_akima_spline_for_xy, const bool use_lerp_for_z, const bool use_zero_order_hold_for_twist, const bool resample_input_trajectory_stop_point) { // validate arguments @@ -632,7 +652,7 @@ autoware_auto_planning_msgs::msg::Trajectory resampleTrajectory( } return resampleTrajectory( - input_trajectory, resampling_arclength, use_lerp_for_xy, use_lerp_for_z, + input_trajectory, resampling_arclength, use_akima_spline_for_xy, use_lerp_for_z, use_zero_order_hold_for_twist); } diff --git a/common/rtc_manager_rviz_plugin/package.xml b/common/rtc_manager_rviz_plugin/package.xml index 8af550e05ac69..6987e63c3c6ee 100644 --- a/common/rtc_manager_rviz_plugin/package.xml +++ b/common/rtc_manager_rviz_plugin/package.xml @@ -5,6 +5,8 @@ 0.0.0 The rtc manager rviz plugin package Taiki Tanaka + Tomoya Kimura + Apache License 2.0 ament_cmake_auto diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp index 8647d87558cb0..2daa837ffd073 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -159,6 +159,9 @@ RTCManagerPanel::RTCManagerPanel(QWidget * parent) : rviz_common::Panel(parent) } v_layout->addWidget(auto_mode_table_); + num_rtc_status_ptr_ = new QLabel("Init"); + v_layout->addWidget(num_rtc_status_ptr_); + // lateral execution auto * exe_path_change_layout = new QHBoxLayout; { @@ -327,6 +330,8 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg { cooperate_statuses_ptr_ = std::make_shared(*msg); rtc_table_->clearContents(); + num_rtc_status_ptr_->setText( + QString::fromStdString("The Number of RTC Statuses: " + std::to_string(msg->statuses.size()))); if (msg->statuses.empty()) return; // this is to stable rtc display not to occupy too much size_t min_display_size{5}; diff --git a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp index 0a16a84b38ca4..8bdaef94b6254 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.hpp @@ -121,6 +121,7 @@ public Q_SLOTS: QPushButton * wait_vel_change_button_ptr_ = {nullptr}; QPushButton * exec_button_ptr_ = {nullptr}; QPushButton * wait_button_ptr_ = {nullptr}; + QLabel * num_rtc_status_ptr_ = {nullptr}; size_t column_size_ = {7}; std::string enable_auto_mode_namespace_ = "/planning/enable_auto_mode"; diff --git a/common/signal_processing/package.xml b/common/signal_processing/package.xml index 9496e87fc8240..df75114decfb0 100644 --- a/common/signal_processing/package.xml +++ b/common/signal_processing/package.xml @@ -12,6 +12,7 @@ Ali BOYALI autoware_cmake + libboost-dev ament_cmake_auto geometry_msgs rclcpp diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index d0a6487db0744..d4a01d67a1732 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -21,8 +21,8 @@ tier4_debug_msgs launch_ros - rclpy python3-rtree + rclpy ament_lint_auto autoware_lint_common diff --git a/common/tier4_planning_rviz_plugin/CMakeLists.txt b/common/tier4_planning_rviz_plugin/CMakeLists.txt index 967e010558eb2..7dc662e52f29f 100644 --- a/common/tier4_planning_rviz_plugin/CMakeLists.txt +++ b/common/tier4_planning_rviz_plugin/CMakeLists.txt @@ -19,20 +19,16 @@ include_directories( ament_auto_add_library(tier4_planning_rviz_plugin SHARED include/drivable_area/display.hpp src/drivable_area/display.cpp + # path point + include/path/display_base.hpp include/path/display.hpp src/path/display.cpp + # footprint + include/path_footprint/display_base.hpp include/path_footprint/display.hpp src/path_footprint/display.cpp - include/path_with_lane_id/display.hpp - src/path_with_lane_id/display.cpp - include/path_with_lane_id_footprint/display.hpp - src/path_with_lane_id_footprint/display.cpp include/pose_with_uuid_stamped/display.hpp src/pose_with_uuid_stamped/display.cpp - include/trajectory/display.hpp - src/trajectory/display.cpp - include/trajectory_footprint/display.hpp - src/trajectory_footprint/display.cpp include/mission_checkpoint/mission_checkpoint.hpp src/mission_checkpoint/mission_checkpoint.cpp src/tools/jsk_overlay_utils.cpp diff --git a/common/tier4_planning_rviz_plugin/include/path/display.hpp b/common/tier4_planning_rviz_plugin/include/path/display.hpp index 0029d13369a3e..01ce426da1985 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -15,71 +15,30 @@ #ifndef PATH__DISPLAY_HPP_ #define PATH__DISPLAY_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include #include - -#include -#include -#include -#include -#include - -#include -#include +#include +#include namespace rviz_plugins { -class AutowarePathDisplay -: public rviz_common::MessageFilterDisplay +class AutowarePathWithLaneIdDisplay +: public AutowarePathBaseDisplay { Q_OBJECT +}; -public: - AutowarePathDisplay(); - ~AutowarePathDisplay() override; - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; - static std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - static std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_{nullptr}; - Ogre::ManualObject * velocity_manual_object_{nullptr}; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: // NOLINT for Qt - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; - static bool validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); +class AutowarePathDisplay : public AutowarePathBaseDisplay +{ + Q_OBJECT }; +class AutowareTrajectoryDisplay +: public AutowarePathBaseDisplay +{ + Q_OBJECT +}; } // namespace rviz_plugins #endif // PATH__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp new file mode 100644 index 0000000000000..567d6a0f1187b --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -0,0 +1,354 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH__DISPLAY_BASE_HPP_ +#define PATH__DISPLAY_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include +#include + +namespace +{ +std::unique_ptr gradation( + const QColor & color_min, const QColor & color_max, const double ratio) +{ + std::unique_ptr color_ptr(new Ogre::ColourValue); + color_ptr->g = + static_cast(color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio)); + color_ptr->r = static_cast(color_max.redF() * ratio + color_min.redF() * (1.0 - ratio)); + color_ptr->b = static_cast(color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio)); + + return color_ptr; +} + +std::unique_ptr setColorDependsOnVelocity( + const double vel_max, const double cmd_vel) +{ + const double cmd_vel_abs = std::fabs(cmd_vel); + const double vel_min = 0.0; + + std::unique_ptr color_ptr(new Ogre::ColourValue()); + if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { + double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); + color_ptr = gradation(Qt::red, Qt::yellow, ratio); + } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { + double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); + color_ptr = gradation(Qt::yellow, Qt::green, ratio); + } else if (vel_max < cmd_vel_abs) { + *color_ptr = Ogre::ColourValue::Green; + } else { + *color_ptr = Ogre::ColourValue::Red; + } + + return color_ptr; +} + +template +bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) +{ + for (auto && path_point : msg_ptr->points) { + if ( + !rviz_common::validateFloats(tier4_autoware_utils::getPose(path_point)) && + !rviz_common::validateFloats(tier4_autoware_utils::getLongitudinalVelocity(path_point))) { + return false; + } + } + return true; +} +} // namespace + +namespace rviz_plugins +{ +template +class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay +{ +public: + AutowarePathBaseDisplay() + : // path + property_path_view_{"View Path", true, "", this}, + property_path_width_{"Width", 2.0, "", &property_path_view_}, + property_path_alpha_{"Alpha", 1.0, "", &property_path_view_}, + property_path_color_view_{"Constant Color", false, "", &property_path_view_}, + property_path_color_{"Color", Qt::black, "", &property_path_view_}, + property_vel_max_{"Color Border Vel Max", 3.0, "[m/s]", this}, + // velocity + property_velocity_view_{"View Velocity", true, "", this}, + property_velocity_alpha_{"Alpha", 1.0, "", &property_velocity_view_}, + property_velocity_scale_{"Scale", 0.3, "", &property_velocity_view_}, + property_velocity_color_view_{"Constant Color", false, "", &property_velocity_view_}, + property_velocity_color_{"Color", Qt::black, "", &property_velocity_view_}, + // velocity text + property_velocity_text_view_{"View Text Velocity", false, "", this}, + property_velocity_text_scale_{"Scale", 0.3, "", &property_velocity_text_view_} + { + // path + property_path_width_.setMin(0.0); + property_path_alpha_.setMin(0.0); + property_path_alpha_.setMax(1.0); + property_vel_max_.setMin(0.0); + // velocity + property_velocity_alpha_.setMin(0.0); + property_velocity_alpha_.setMax(1.0); + // velocity text + property_velocity_scale_.setMin(0.1); + property_velocity_scale_.setMax(10.0); + } + + ~AutowarePathBaseDisplay() override + { + if (this->initialized()) { + this->scene_manager_->destroyManualObject(path_manual_object_); + this->scene_manager_->destroyManualObject(velocity_manual_object_); + for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + this->scene_manager_->destroySceneNode(node); + } + } + } + void onInitialize() override + { + rviz_common::MessageFilterDisplay::MFDClass::onInitialize(); + + path_manual_object_ = this->scene_manager_->createManualObject(); + velocity_manual_object_ = this->scene_manager_->createManualObject(); + path_manual_object_->setDynamic(true); + velocity_manual_object_->setDynamic(true); + this->scene_node_->attachObject(path_manual_object_); + this->scene_node_->attachObject(velocity_manual_object_); + } + void reset() override + { + rviz_common::MessageFilterDisplay::MFDClass::reset(); + path_manual_object_->clear(); + velocity_manual_object_->clear(); + for (size_t i = 0; i < velocity_texts_.size(); i++) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + velocity_text_nodes_.clear(); + velocity_texts_.clear(); + } + +protected: + void processMessage(const typename T::ConstSharedPtr msg_ptr) override + { + if (!validateFloats(msg_ptr)) { + this->setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + rclcpp::get_logger("AutowarePathBaseDisplay"), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(this->fixed_frame_)); + } + + this->scene_node_->setPosition(position); + this->scene_node_->setOrientation(orientation); + + path_manual_object_->clear(); + velocity_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); + velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); + path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); + // path_manual_object_->begin("BaseWhiteNoLighting", + // Ogre::RenderOperation::OT_TRIANGLE_STRIP); + velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); + + if (msg_ptr->points.size() > velocity_texts_.size()) { + for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { + Ogre::SceneNode * node = this->scene_node_->createChildSceneNode(); + rviz_rendering::MovableText * text = + new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); + text->setVisible(false); + text->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node->attachObject(text); + velocity_texts_.push_back(text); + velocity_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < velocity_texts_.size()) { + for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = velocity_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + velocity_texts_.resize(msg_ptr->points.size()); + velocity_text_nodes_.resize(msg_ptr->points.size()); + } + + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { + const auto & path_point = msg_ptr->points.at(point_idx); + const auto & pose = tier4_autoware_utils::getPose(path_point); + const auto & velocity = tier4_autoware_utils::getLongitudinalVelocity(path_point); + + // path + if (property_path_view_.getBool()) { + Ogre::ColourValue color; + if (property_path_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_path_color_.getColor()); + } else { + // color change depending on velocity + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_path_alpha_.getFloat(); + Eigen::Vector3f vec_in; + Eigen::Vector3f vec_out; + Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); + { + vec_in << 0, (property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); + } + { + vec_in << 0, -(property_path_width_.getFloat() / 2.0), 0; + Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + if (!isDrivingForward(msg_ptr->points, point_idx)) { + quat *= quat_yaw_reverse; + } + vec_out = quat * vec_in; + path_manual_object_->position( + static_cast(pose.position.x) + vec_out.x(), + static_cast(pose.position.y) + vec_out.y(), + static_cast(pose.position.z) + vec_out.z()); + path_manual_object_->colour(color); + } + } + + // velocity + if (property_velocity_view_.getBool()) { + Ogre::ColourValue color; + if (property_velocity_color_view_.getBool()) { + color = rviz_common::properties::qtToOgre(property_velocity_color_.getColor()); + } else { + /* color change depending on velocity */ + std::unique_ptr dynamic_color_ptr = + setColorDependsOnVelocity(property_vel_max_.getFloat(), velocity); + color = *dynamic_color_ptr; + } + color.a = property_velocity_alpha_.getFloat(); + + velocity_manual_object_->position( + pose.position.x, pose.position.y, + static_cast(pose.position.z) + velocity * property_velocity_scale_.getFloat()); + velocity_manual_object_->colour(color); + } + + // velocity text + if (property_velocity_text_view_.getBool()) { + Ogre::Vector3 position; + position.x = pose.position.x; + position.y = pose.position.y; + position.z = pose.position.z; + Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + const double vel = velocity; + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << vel; + text->setCaption(ss.str()); + text->setCharacterHeight(property_velocity_text_scale_.getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); + text->setVisible(false); + } + } + + path_manual_object_->end(); + velocity_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; + } + + Ogre::ManualObject * path_manual_object_{nullptr}; + Ogre::ManualObject * velocity_manual_object_{nullptr}; + std::vector velocity_texts_; + std::vector velocity_text_nodes_; + + rviz_common::properties::BoolProperty property_path_view_; + rviz_common::properties::FloatProperty property_path_width_; + rviz_common::properties::FloatProperty property_path_alpha_; + rviz_common::properties::BoolProperty property_path_color_view_; + rviz_common::properties::ColorProperty property_path_color_; + rviz_common::properties::FloatProperty property_vel_max_; + rviz_common::properties::BoolProperty property_velocity_view_; + rviz_common::properties::FloatProperty property_velocity_alpha_; + rviz_common::properties::FloatProperty property_velocity_scale_; + rviz_common::properties::BoolProperty property_velocity_color_view_; + rviz_common::properties::ColorProperty property_velocity_color_; + rviz_common::properties::BoolProperty property_velocity_text_view_; + rviz_common::properties::FloatProperty property_velocity_text_scale_; + +private: + typename T::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // PATH__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp index 21442120b0062..1db170dd0cd46 100644 --- a/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/path_footprint/display.hpp @@ -15,76 +15,53 @@ #ifndef PATH_FOOTPRINT__DISPLAY_HPP_ #define PATH_FOOTPRINT__DISPLAY_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include - -#include -#include -#include -#include -#include +#include +#include #include +#include +#include namespace rviz_plugins { -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowarePathFootprintDisplay -: public rviz_common::MessageFilterDisplay +class AutowarePathWithLaneIdFootprintDisplay +: public AutowarePathFootBaseprintDisplay { Q_OBJECT public: - AutowarePathFootprintDisplay(); - virtual ~AutowarePathFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * path_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_path_footprint_view_; - rviz_common::properties::ColorProperty * property_path_footprint_color_; - rviz_common::properties::FloatProperty * property_path_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; + AutowarePathWithLaneIdFootprintDisplay(); private: - autoware_auto_planning_msgs::msg::Path::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr); + void resetDetail() override; + void preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; + void processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) override; + + rviz_common::properties::BoolProperty property_lane_id_view_; + rviz_common::properties::FloatProperty property_lane_id_scale_; + + using LaneIdObject = + std::pair, std::unique_ptr>; + std::vector lane_id_obj_ptrs_; +}; +class AutowarePathFootprintDisplay +: public AutowarePathFootBaseprintDisplay +{ + Q_OBJECT }; +class AutowareTrajectoryFootprintDisplay +: public AutowarePathFootBaseprintDisplay +{ + Q_OBJECT +}; } // namespace rviz_plugins #endif // PATH_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp new file mode 100644 index 0000000000000..1b83b7c3258f3 --- /dev/null +++ b/common/tier4_planning_rviz_plugin/include/path_footprint/display_base.hpp @@ -0,0 +1,315 @@ +// Copyright 2021 Tier IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PATH_FOOTPRINT__DISPLAY_BASE_HPP_ +#define PATH_FOOTPRINT__DISPLAY_BASE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace rviz_plugins +{ +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + +template +class AutowarePathFootBaseprintDisplay : public rviz_common::MessageFilterDisplay +{ +public: + AutowarePathFootBaseprintDisplay() + : // footprint + property_footprint_view_{"View Footprint", true, "", this}, + property_footprint_alpha_{"Alpha", 1.0, "", &property_footprint_view_}, + property_footprint_color_{"Color", QColor(230, 230, 50), "", &property_footprint_view_}, + property_vehicle_length_{"Vehicle Length", 4.77, "", &property_footprint_view_}, + property_vehicle_width_{"Vehicle Width", 1.83, "", &property_footprint_view_}, + property_rear_overhang_{"Rear Overhang", 1.03, "", &property_footprint_view_}, + property_offset_{"Offset from BaseLink", 0.0, "", &property_footprint_view_}, + // point + property_point_view_{"View Point", false, "", this}, + property_point_alpha_{"Alpha", 1.0, "", &property_point_view_}, + property_point_color_{"Color", QColor(0, 60, 255), "", &property_point_view_}, + property_point_radius_{"Radius", 0.1, "", &property_point_view_}, + property_point_offset_{"Offset", 0.0, "", &property_point_view_} + { + // initialize footprint + property_footprint_alpha_.setMin(0.0); + property_footprint_alpha_.setMax(1.0); + property_vehicle_length_.setMin(0.0); + property_vehicle_width_.setMin(0.0); + property_rear_overhang_.setMin(0.0); + // initialize point + property_point_alpha_.setMin(0.0); + property_point_alpha_.setMax(1.0); + + updateVehicleInfo(); + } + + virtual ~AutowarePathFootBaseprintDisplay() + { + if (this->initialized()) { + this->scene_manager_->destroyManualObject(footprint_manual_object_); + this->scene_manager_->destroyManualObject(point_manual_object_); + } + } + + void onInitialize() override + { + rviz_common::MessageFilterDisplay::MFDClass::onInitialize(); + + footprint_manual_object_ = this->scene_manager_->createManualObject(); + footprint_manual_object_->setDynamic(true); + this->scene_node_->attachObject(footprint_manual_object_); + + point_manual_object_ = this->scene_manager_->createManualObject(); + point_manual_object_->setDynamic(true); + this->scene_node_->attachObject(point_manual_object_); + } + + void reset() override + { + rviz_common::MessageFilterDisplay::MFDClass::reset(); + footprint_manual_object_->clear(); + point_manual_object_->clear(); + + resetDetail(); + } + +protected: + virtual void resetDetail() {} + virtual void preprocessMessageDetail([[maybe_unused]] const typename T::ConstSharedPtr msg_ptr) {} + virtual void processMessageDetail( + [[maybe_unused]] const typename T::ConstSharedPtr msg_ptr, [[maybe_unused]] const size_t p_idx) + { + } + + Ogre::ManualObject * footprint_manual_object_; + rviz_common::properties::BoolProperty property_footprint_view_; + rviz_common::properties::FloatProperty property_footprint_alpha_; + rviz_common::properties::ColorProperty property_footprint_color_; + rviz_common::properties::FloatProperty property_vehicle_length_; + rviz_common::properties::FloatProperty property_vehicle_width_; + rviz_common::properties::FloatProperty property_rear_overhang_; + rviz_common::properties::FloatProperty property_offset_; + + Ogre::ManualObject * point_manual_object_; + rviz_common::properties::BoolProperty property_point_view_; + rviz_common::properties::FloatProperty property_point_alpha_; + rviz_common::properties::ColorProperty property_point_color_; + rviz_common::properties::FloatProperty property_point_radius_; + rviz_common::properties::FloatProperty property_point_offset_; + + void updateVisualization() + { + if (last_msg_ptr_ != nullptr) { + processMessage(last_msg_ptr_); + } + } + +private: + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + + std::shared_ptr vehicle_info_; + std::shared_ptr vehicle_footprint_info_; + + void updateVehicleInfo() + { + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + const float length{property_vehicle_length_.getFloat()}; + const float width{property_vehicle_width_.getFloat()}; + const float rear_overhang{property_rear_overhang_.getFloat()}; + + vehicle_footprint_info_ = + std::make_shared(length, width, rear_overhang); + } + } + + typename T::ConstSharedPtr last_msg_ptr_; + bool validateFloats(const typename T::ConstSharedPtr & msg_ptr) + { + for (auto && point : msg_ptr->points) { + if (!rviz_common::validateFloats(tier4_autoware_utils::getPose(point))) { + return false; + } + } + return true; + } + + void processMessage(const typename T::ConstSharedPtr msg_ptr) override + { + if (!validateFloats(msg_ptr)) { + this->setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*this->rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *this->rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, + "Failed to get vehicle_info: %s", e.what()); + } + } + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!this->context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { + RCLCPP_DEBUG( + this->rviz_ros_node_.lock()->get_raw_node()->get_logger(), + "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), + qPrintable(this->fixed_frame_)); + } + + this->scene_node_->setPosition(position); + this->scene_node_->setOrientation(orientation); + + footprint_manual_object_->clear(); + point_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!msg_ptr->points.empty()) { + footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); + footprint_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); + point_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + + const float offset_from_baselink = property_offset_.getFloat(); + + preprocessMessageDetail(msg_ptr); + + for (size_t p_idx = 0; p_idx < msg_ptr->points.size(); p_idx++) { + const auto & point = msg_ptr->points.at(p_idx); + const auto & pose = tier4_autoware_utils::getPose(point); + // footprint + if (property_footprint_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_footprint_color_.getColor()); + color.a = property_footprint_alpha_.getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const Eigen::Quaternionf quat( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); + } + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + footprint_manual_object_->position( + pose.position.x + offset_to_edge.x(), pose.position.y + offset_to_edge.y(), + pose.position.z); + footprint_manual_object_->colour(color); + } + } + } + + // point + if (property_point_view_.getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_point_color_.getColor()); + color.a = property_point_alpha_.getFloat(); + + const double offset = property_point_offset_.getFloat(); + const double yaw = tf2::getYaw(pose.orientation); + const double base_x = pose.position.x + offset * std::cos(yaw); + const double base_y = pose.position.y + offset * std::sin(yaw); + const double base_z = pose.position.z; + + const double radius = property_point_radius_.getFloat(); + for (size_t s_idx = 0; s_idx < 8; ++s_idx) { + const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; + const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; + point_manual_object_->position( + base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), + base_z); + point_manual_object_->colour(color); + + point_manual_object_->position( + base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), + base_z); + point_manual_object_->colour(color); + + point_manual_object_->position(base_x, base_y, base_z); + point_manual_object_->colour(color); + } + } + + processMessageDetail(msg_ptr, p_idx); + } + + footprint_manual_object_->end(); + point_manual_object_->end(); + } + last_msg_ptr_ = msg_ptr; + } +}; +} // namespace rviz_plugins + +#endif // PATH_FOOTPRINT__DISPLAY_BASE_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp deleted file mode 100644 index df01ffc73c466..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id/display.hpp +++ /dev/null @@ -1,91 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH_WITH_LANE_ID__DISPLAY_HPP_ -#define PATH_WITH_LANE_ID__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -class AutowarePathWithLaneIdDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowarePathWithLaneIdDisplay(); - virtual ~AutowarePathWithLaneIdDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_; - Ogre::ManualObject * velocity_manual_object_; - std::vector velocity_texts_; - std::vector velocity_text_nodes_; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_velocity_text_view_; - rviz_common::properties::FloatProperty * property_velocity_text_scale_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: - autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_; - bool validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // PATH_WITH_LANE_ID__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp deleted file mode 100644 index 4cbcb9a47daa2..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/path_with_lane_id_footprint/display.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ -#define PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowarePathWithLaneIdFootprintDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowarePathWithLaneIdFootprintDisplay(); - virtual ~AutowarePathWithLaneIdFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * path_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_path_footprint_view_; - rviz_common::properties::ColorProperty * property_path_footprint_color_; - rviz_common::properties::FloatProperty * property_path_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - rviz_common::properties::BoolProperty * property_lane_id_view_; - rviz_common::properties::FloatProperty * property_lane_id_scale_; - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; - - using LaneIdObject = - std::pair, std::unique_ptr>; - std::vector lane_id_obj_ptrs_; - -private: - autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr last_msg_ptr_; - bool validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr); - - void allocateLaneIdObjects(const std::size_t size); -}; - -} // namespace rviz_plugins - -#endif // PATH_WITH_LANE_ID_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp deleted file mode 100644 index 428774e1d3b90..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/trajectory/display.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAJECTORY__DISPLAY_HPP_ -#define TRAJECTORY__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace rviz_plugins -{ -class AutowareTrajectoryDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowareTrajectoryDisplay(); - virtual ~AutowareTrajectoryDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; - std::unique_ptr setColorDependsOnVelocity( - const double vel_max, const double cmd_vel); - std::unique_ptr gradation( - const QColor & color_min, const QColor & color_max, const double ratio); - Ogre::ManualObject * path_manual_object_; - Ogre::ManualObject * velocity_manual_object_; - std::vector velocity_texts_; - std::vector velocity_text_nodes_; - rviz_common::properties::BoolProperty * property_path_view_; - rviz_common::properties::BoolProperty * property_velocity_view_; - rviz_common::properties::FloatProperty * property_path_width_; - rviz_common::properties::ColorProperty * property_path_color_; - rviz_common::properties::ColorProperty * property_velocity_color_; - rviz_common::properties::FloatProperty * property_velocity_scale_; - rviz_common::properties::BoolProperty * property_velocity_text_view_; - rviz_common::properties::FloatProperty * property_velocity_text_scale_; - rviz_common::properties::FloatProperty * property_path_alpha_; - rviz_common::properties::FloatProperty * property_velocity_alpha_; - rviz_common::properties::BoolProperty * property_path_color_view_; - rviz_common::properties::BoolProperty * property_velocity_color_view_; - rviz_common::properties::FloatProperty * property_vel_max_; - -private: - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // TRAJECTORY__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp deleted file mode 100644 index d3964f9e7ce07..0000000000000 --- a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ -#define TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include - -namespace rviz_plugins -{ -using vehicle_info_util::VehicleInfo; -using vehicle_info_util::VehicleInfoUtil; - -class AutowareTrajectoryFootprintDisplay -: public rviz_common::MessageFilterDisplay -{ - Q_OBJECT - -public: - AutowareTrajectoryFootprintDisplay(); - virtual ~AutowareTrajectoryFootprintDisplay(); - - void onInitialize() override; - void reset() override; - -private Q_SLOTS: - void updateVisualization(); - void updateVehicleInfo(); - -protected: - void processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) override; - Ogre::ManualObject * trajectory_footprint_manual_object_; - rviz_common::properties::BoolProperty * property_trajectory_footprint_view_; - rviz_common::properties::ColorProperty * property_trajectory_footprint_color_; - rviz_common::properties::FloatProperty * property_trajectory_footprint_alpha_; - rviz_common::properties::FloatProperty * property_vehicle_length_; - rviz_common::properties::FloatProperty * property_vehicle_width_; - rviz_common::properties::FloatProperty * property_rear_overhang_; - rviz_common::properties::FloatProperty * property_offset_; - - Ogre::ManualObject * trajectory_point_manual_object_; - rviz_common::properties::BoolProperty * property_trajectory_point_view_; - rviz_common::properties::ColorProperty * property_trajectory_point_color_; - rviz_common::properties::FloatProperty * property_trajectory_point_alpha_; - rviz_common::properties::FloatProperty * property_trajectory_point_radius_; - rviz_common::properties::FloatProperty * property_trajectory_point_offset_; - - struct VehicleFootprintInfo - { - VehicleFootprintInfo(const float l, const float w, const float r) - : length(l), width(w), rear_overhang(r) - { - } - float length, width, rear_overhang; - }; - std::shared_ptr vehicle_info_; - std::shared_ptr vehicle_footprint_info_; - -private: - autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_msg_ptr_; - bool validateFloats(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr); -}; - -} // namespace rviz_plugins - -#endif // TRAJECTORY_FOOTPRINT__DISPLAY_HPP_ diff --git a/common/tier4_planning_rviz_plugin/src/path/display.cpp b/common/tier4_planning_rviz_plugin/src/path/display.cpp index 02d6623cc5b6e..b4e61616e18e3 100644 --- a/common/tier4_planning_rviz_plugin/src/path/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path/display.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -13,248 +13,7 @@ // limitations under the License. #include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowarePathDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = - static_cast(color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio)); - color_ptr->r = static_cast(color_max.redF() * ratio + color_min.redF() * (1.0 - ratio)); - color_ptr->b = static_cast(color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio)); - - return color_ptr; -} - -std::unique_ptr AutowarePathDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowarePathDisplay::AutowarePathDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowarePathDisplay::~AutowarePathDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - } -} - -void AutowarePathDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowarePathDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); -} - -bool AutowarePathDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if ( - !rviz_common::validateFloats(path_point.pose) && - !rviz_common::validateFloats(path_point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowarePathDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowarePathDisplay"), "Error transforming from frame '%s' to frame '%s'", - msg_ptr->header.frame_id.c_str(), qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in; - Eigen::Vector3f vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(path_point.pose.position.x) + vec_out.x(), - static_cast(path_point.pose.position.y) + vec_out.y(), - static_cast(path_point.pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - static_cast(path_point.pose.position.x) + vec_out.x(), - static_cast(path_point.pose.position.y) + vec_out.y(), - static_cast(path_point.pose.position.z) + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - path_point.pose.position.x, path_point.pose.position.y, - static_cast(path_point.pose.position.z) + - path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - #include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp index 5e58d6da03628..1124cde14da15 100644 --- a/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/path_footprint/display.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. +// Copyright 2023 TIER IV, Inc. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -18,188 +18,80 @@ #include #include #include +#include namespace rviz_plugins { -AutowarePathFootprintDisplay::AutowarePathFootprintDisplay() +AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay() +: property_lane_id_view_{"View LaneId", true, "", this}, + property_lane_id_scale_{"Scale", 0.1, "", &property_lane_id_view_} { - property_path_footprint_view_ = new rviz_common::properties::BoolProperty( - "View Path Footprint", true, "", this, SLOT(updateVisualization()), this); - property_path_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_footprint_view_, SLOT(updateVisualization()), this); - property_path_footprint_alpha_->setMin(0.0); - property_path_footprint_alpha_->setMax(1.0); - property_path_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_path_footprint_view_, SLOT(updateVisualization()), - this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - updateVehicleInfo(); } -AutowarePathFootprintDisplay::~AutowarePathFootprintDisplay() +void AutowarePathWithLaneIdFootprintDisplay::resetDetail() { - if (initialized()) { - scene_manager_->destroyManualObject(path_footprint_manual_object_); + for (const auto & e : lane_id_obj_ptrs_) { + scene_node_->removeChild(e.first.get()); } + lane_id_obj_ptrs_.clear(); + lane_id_obj_ptrs_.shrink_to_fit(); } -void AutowarePathFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_footprint_manual_object_ = scene_manager_->createManualObject(); - path_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(path_footprint_manual_object_); -} - -void AutowarePathFootprintDisplay::reset() +void AutowarePathWithLaneIdFootprintDisplay::preprocessMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) { - MFDClass::reset(); - path_footprint_manual_object_->clear(); -} - -bool AutowarePathFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if (!rviz_common::validateFloats(path_point.pose)) { - return false; + const size_t size = msg_ptr->points.size(); + if (size > lane_id_obj_ptrs_.size()) { + for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { + std::unique_ptr node_ptr; + node_ptr.reset(scene_node_->createChildSceneNode()); + auto text_ptr = + std::make_unique("not initialized", "Liberation Sans", 0.1); + text_ptr->setVisible(false); + text_ptr->setTextAlignment( + rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); + node_ptr->attachObject(text_ptr.get()); + lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); } - } - return true; -} - -void AutowarePathFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_footprint_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - path_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_path_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_path_footprint_color_->getColor()); - color.a = property_path_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - } - } + } else { + for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { + scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); } - - path_footprint_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); + lane_id_obj_ptrs_.resize(size); } } -void AutowarePathFootprintDisplay::updateVehicleInfo() +void AutowarePathWithLaneIdFootprintDisplay::processMessageDetail( + const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr, + const size_t p_idx) { - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); + const auto & point = msg_ptr->points.at(p_idx); + + // LaneId + if (property_lane_id_view_.getBool()) { + Ogre::Vector3 position; + position.x = point.point.pose.position.x; + position.y = point.point.pose.position.y; + position.z = point.point.pose.position.z; + auto & node_ptr = lane_id_obj_ptrs_.at(p_idx).first; + node_ptr->setPosition(position); + + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + std::string lane_ids_str = ""; + for (const auto & e : point.lane_ids) { + lane_ids_str += std::to_string(e) + ", "; + } + text_ptr->setCaption(lane_ids_str); + text_ptr->setCharacterHeight(property_lane_id_scale_.getFloat()); + text_ptr->setVisible(true); } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + const auto & text_ptr = lane_id_obj_ptrs_.at(p_idx).second; + text_ptr->setVisible(false); } } - } // namespace rviz_plugins -#include +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display) PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathFootprintDisplay, rviz_common::Display) +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp deleted file mode 100644 index 64400197fbbb7..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id/display.cpp +++ /dev/null @@ -1,312 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowarePathWithLaneIdDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); - color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); - color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); - - return color_ptr; -} - -std::unique_ptr AutowarePathWithLaneIdDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowarePathWithLaneIdDisplay::AutowarePathWithLaneIdDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_text_view_ = new rviz_common::properties::BoolProperty( - "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); - property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowarePathWithLaneIdDisplay::~AutowarePathWithLaneIdDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->removeAndDestroyAllChildren(); - node->detachAllObjects(); - scene_manager_->destroySceneNode(node); - } - } -} - -void AutowarePathWithLaneIdDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowarePathWithLaneIdDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); -} - -bool AutowarePathWithLaneIdDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr) -{ - for (auto && e : msg_ptr->points) { - if ( - !rviz_common::validateFloats(e.point.pose) && - !rviz_common::validateFloats(e.point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowarePathWithLaneIdDisplay::processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowarePathWithLaneIdDisplay"), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - // path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - if (msg_ptr->points.size() > velocity_texts_.size()) { - for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { - Ogre::SceneNode * node = scene_node_->createChildSceneNode(); - rviz_rendering::MovableText * text = - new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); - text->setVisible(false); - text->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node->attachObject(text); - velocity_texts_.push_back(text); - velocity_text_nodes_.push_back(node); - } - } else if (msg_ptr->points.size() < velocity_texts_.size()) { - for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_texts_.resize(msg_ptr->points.size()); - velocity_text_nodes_.resize(msg_ptr->points.size()); - } - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & e = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in, vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y, - e.point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(), - e.point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - e.point.pose.orientation.w, e.point.pose.orientation.x, e.point.pose.orientation.y, - e.point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - e.point.pose.position.x + vec_out.x(), e.point.pose.position.y + vec_out.y(), - e.point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), e.point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - e.point.pose.position.x, e.point.pose.position.y, - e.point.pose.position.z + - e.point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - - /* - * Velocity Text - */ - if (property_velocity_text_view_->getBool()) { - Ogre::Vector3 position; - position.x = e.point.pose.position.x; - position.y = e.point.pose.position.y; - position.z = e.point.pose.position.z; - Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); - node->setPosition(position); - - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - const double vel = e.point.longitudinal_velocity_mps; - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << vel; - text->setCaption(ss.str()); - text->setCharacterHeight(property_velocity_text_scale_->getFloat()); - text->setVisible(true); - } else { - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - text->setVisible(false); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathWithLaneIdDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - -#include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp deleted file mode 100644 index c1eb332b4332d..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/path_with_lane_id_footprint/display.cpp +++ /dev/null @@ -1,264 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#define EIGEN_MPL2_ONLY -#include -#include -#include - -namespace rviz_plugins -{ -AutowarePathWithLaneIdFootprintDisplay::AutowarePathWithLaneIdFootprintDisplay() -{ - property_path_footprint_view_ = new rviz_common::properties::BoolProperty( - "View PathWithLaneId Footprint", true, "", this, SLOT(updateVisualization()), this); - property_path_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_footprint_view_, SLOT(updateVisualization()), this); - property_path_footprint_alpha_->setMin(0.0); - property_path_footprint_alpha_->setMax(1.0); - property_path_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_path_footprint_view_, SLOT(updateVisualization()), - this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_path_footprint_view_, SLOT(updateVehicleInfo()), this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - property_lane_id_view_ = new rviz_common::properties::BoolProperty( - "View LaneId", true, "", this, SLOT(updateVisualization()), this); - property_lane_id_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.1, "", property_lane_id_view_, SLOT(updateVisualization()), this); - - updateVehicleInfo(); -} - -AutowarePathWithLaneIdFootprintDisplay::~AutowarePathWithLaneIdFootprintDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_footprint_manual_object_); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_footprint_manual_object_ = scene_manager_->createManualObject(); - path_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(path_footprint_manual_object_); -} - -void AutowarePathWithLaneIdFootprintDisplay::reset() -{ - MFDClass::reset(); - path_footprint_manual_object_->clear(); - - for (const auto & e : lane_id_obj_ptrs_) { - scene_node_->removeChild(e.first.get()); - } - lane_id_obj_ptrs_.clear(); - lane_id_obj_ptrs_.shrink_to_fit(); -} - -bool AutowarePathWithLaneIdFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr & msg_ptr) -{ - for (auto && path_point : msg_ptr->points) { - if (!rviz_common::validateFloats(path_point.point.pose)) { - return false; - } - } - return true; -} - -void AutowarePathWithLaneIdFootprintDisplay::allocateLaneIdObjects(const std::size_t size) -{ - if (size > lane_id_obj_ptrs_.size()) { - for (std::size_t i = lane_id_obj_ptrs_.size(); i < size; i++) { - std::unique_ptr node_ptr; - node_ptr.reset(scene_node_->createChildSceneNode()); - auto text_ptr = - std::make_unique("not initialized", "Liberation Sans", 0.1); - text_ptr->setVisible(false); - text_ptr->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node_ptr->attachObject(text_ptr.get()); - lane_id_obj_ptrs_.push_back(std::make_pair(std::move(node_ptr), std::move(text_ptr))); - } - } else { - for (std::size_t i = lane_id_obj_ptrs_.size() - 1; i >= size; i--) { - scene_node_->removeChild(lane_id_obj_ptrs_.at(i).first.get()); - } - lane_id_obj_ptrs_.resize(size); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_footprint_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - path_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - allocateLaneIdObjects(msg_ptr->points.size()); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_path_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_path_footprint_color_->getColor()); - color.a = property_path_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.point.pose.orientation.w, path_point.point.pose.orientation.x, - path_point.point.pose.orientation.y, path_point.point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.point.pose.position.x + offset_to_edge.x(), - path_point.point.pose.position.y + offset_to_edge.y(), - path_point.point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - path_footprint_manual_object_->position( - path_point.point.pose.position.x + offset_to_edge.x(), - path_point.point.pose.position.y + offset_to_edge.y(), - path_point.point.pose.position.z); - path_footprint_manual_object_->colour(color); - } - } - } - - // LaneId - if (property_lane_id_view_->getBool()) { - Ogre::Vector3 position; - position.x = path_point.point.pose.position.x; - position.y = path_point.point.pose.position.y; - position.z = path_point.point.pose.position.z; - auto & node_ptr = lane_id_obj_ptrs_.at(point_idx).first; - node_ptr->setPosition(position); - - const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second; - std::string lane_ids_str = ""; - for (const auto & e : path_point.lane_ids) { - lane_ids_str += std::to_string(e) + ", "; - } - text_ptr->setCaption(lane_ids_str); - text_ptr->setCharacterHeight(property_lane_id_scale_->getFloat()); - text_ptr->setVisible(true); - } else { - const auto & text_ptr = lane_id_obj_ptrs_.at(point_idx).second; - text_ptr->setVisible(false); - } - } - - path_footprint_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowarePathWithLaneIdFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -void AutowarePathWithLaneIdFootprintDisplay::updateVehicleInfo() -{ - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowarePathWithLaneIdFootprintDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp deleted file mode 100644 index 558063ca6a032..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/trajectory/display.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include -#define EIGEN_MPL2_ONLY -#include -#include - -namespace rviz_plugins -{ -std::unique_ptr AutowareTrajectoryDisplay::gradation( - const QColor & color_min, const QColor & color_max, const double ratio) -{ - std::unique_ptr color_ptr(new Ogre::ColourValue); - color_ptr->g = color_max.greenF() * ratio + color_min.greenF() * (1.0 - ratio); - color_ptr->r = color_max.redF() * ratio + color_min.redF() * (1.0 - ratio); - color_ptr->b = color_max.blueF() * ratio + color_min.blueF() * (1.0 - ratio); - - return color_ptr; -} - -std::unique_ptr AutowareTrajectoryDisplay::setColorDependsOnVelocity( - const double vel_max, const double cmd_vel) -{ - const double cmd_vel_abs = std::fabs(cmd_vel); - const double vel_min = 0.0; - - std::unique_ptr color_ptr(new Ogre::ColourValue()); - if (vel_min < cmd_vel_abs && cmd_vel_abs <= (vel_max / 2.0)) { - double ratio = (cmd_vel_abs - vel_min) / (vel_max / 2.0 - vel_min); - color_ptr = gradation(Qt::red, Qt::yellow, ratio); - } else if ((vel_max / 2.0) < cmd_vel_abs && cmd_vel_abs <= vel_max) { - double ratio = (cmd_vel_abs - vel_max / 2.0) / (vel_max - vel_max / 2.0); - color_ptr = gradation(Qt::yellow, Qt::green, ratio); - } else if (vel_max < cmd_vel_abs) { - *color_ptr = Ogre::ColourValue::Green; - } else { - *color_ptr = Ogre::ColourValue::Red; - } - - return color_ptr; -} - -AutowareTrajectoryDisplay::AutowareTrajectoryDisplay() -{ - property_path_view_ = new rviz_common::properties::BoolProperty( - "View Path", true, "", this, SLOT(updateVisualization())); - property_path_width_ = new rviz_common::properties::FloatProperty( - "Width", 2.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_width_->setMin(0.0); - property_path_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_alpha_->setMin(0.0); - property_path_alpha_->setMax(1.0); - property_path_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_path_view_, SLOT(updateVisualization()), this); - property_path_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_path_view_, SLOT(updateVisualization()), this); - - property_velocity_view_ = new rviz_common::properties::BoolProperty( - "View Velocity", true, "", this, SLOT(updateVisualization()), this); - property_velocity_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_alpha_->setMin(0.0); - property_velocity_alpha_->setMax(1.0); - property_velocity_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_scale_->setMin(0.1); - property_velocity_scale_->setMax(10.0); - property_velocity_color_view_ = new rviz_common::properties::BoolProperty( - "Constant Color", false, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_color_ = new rviz_common::properties::ColorProperty( - "Color", Qt::black, "", property_velocity_view_, SLOT(updateVisualization()), this); - property_velocity_text_view_ = new rviz_common::properties::BoolProperty( - "View Text Velocity", false, "", this, SLOT(updateVisualization()), this); - property_velocity_text_scale_ = new rviz_common::properties::FloatProperty( - "Scale", 0.3, "", property_velocity_text_view_, SLOT(updateVisualization()), this); - property_vel_max_ = new rviz_common::properties::FloatProperty( - "Color Border Vel Max", 3.0, "[m/s]", this, SLOT(updateVisualization()), this); - property_vel_max_->setMin(0.0); -} - -AutowareTrajectoryDisplay::~AutowareTrajectoryDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(path_manual_object_); - scene_manager_->destroyManualObject(velocity_manual_object_); - for (size_t i = 0; i < velocity_text_nodes_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->removeAndDestroyAllChildren(); - node->detachAllObjects(); - scene_manager_->destroySceneNode(node); - } - } -} - -void AutowareTrajectoryDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - path_manual_object_ = scene_manager_->createManualObject(); - velocity_manual_object_ = scene_manager_->createManualObject(); - path_manual_object_->setDynamic(true); - velocity_manual_object_->setDynamic(true); - scene_node_->attachObject(path_manual_object_); - scene_node_->attachObject(velocity_manual_object_); -} - -void AutowareTrajectoryDisplay::reset() -{ - MFDClass::reset(); - path_manual_object_->clear(); - velocity_manual_object_->clear(); - for (size_t i = 0; i < velocity_texts_.size(); i++) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_text_nodes_.clear(); - velocity_texts_.clear(); -} - -bool AutowareTrajectoryDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) -{ - for (auto && trajectory_point : msg_ptr->points) { - if ( - !rviz_common::validateFloats(trajectory_point.pose) && - !rviz_common::validateFloats(trajectory_point.longitudinal_velocity_mps)) { - return false; - } - } - return true; -} - -void AutowareTrajectoryDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rclcpp::get_logger("AutowareTrajectoryDisplay"), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - path_manual_object_->clear(); - velocity_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - path_manual_object_->estimateVertexCount(msg_ptr->points.size() * 2); - velocity_manual_object_->estimateVertexCount(msg_ptr->points.size()); - path_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_STRIP); - velocity_manual_object_->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP); - - if (msg_ptr->points.size() > velocity_texts_.size()) { - for (size_t i = velocity_texts_.size(); i < msg_ptr->points.size(); i++) { - Ogre::SceneNode * node = scene_node_->createChildSceneNode(); - rviz_rendering::MovableText * text = - new rviz_rendering::MovableText("not initialized", "Liberation Sans", 0.1); - text->setVisible(false); - text->setTextAlignment( - rviz_rendering::MovableText::H_CENTER, rviz_rendering::MovableText::V_ABOVE); - node->attachObject(text); - velocity_texts_.push_back(text); - velocity_text_nodes_.push_back(node); - } - } else if (msg_ptr->points.size() < velocity_texts_.size()) { - for (size_t i = velocity_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { - Ogre::SceneNode * node = velocity_text_nodes_.at(i); - node->detachAllObjects(); - node->removeAndDestroyAllChildren(); - scene_manager_->destroySceneNode(node); - } - velocity_texts_.resize(msg_ptr->points.size()); - velocity_text_nodes_.resize(msg_ptr->points.size()); - } - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Path - */ - if (property_path_view_->getBool()) { - Ogre::ColourValue color; - if (property_path_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_path_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_path_alpha_->getFloat(); - Eigen::Vector3f vec_in, vec_out; - Eigen::Quaternionf quat_yaw_reverse(0, 0, 0, 1); - { - vec_in << 0, (property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - { - vec_in << 0, -(property_path_width_->getFloat() / 2.0), 0; - Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - if (!isDrivingForward(msg_ptr->points, point_idx)) { - quat *= quat_yaw_reverse; - } - vec_out = quat * vec_in; - path_manual_object_->position( - path_point.pose.position.x + vec_out.x(), path_point.pose.position.y + vec_out.y(), - path_point.pose.position.z + vec_out.z()); - path_manual_object_->colour(color); - } - } - /* - * Velocity - */ - if (property_velocity_view_->getBool()) { - Ogre::ColourValue color; - if (property_velocity_color_view_->getBool()) { - color = rviz_common::properties::qtToOgre(property_velocity_color_->getColor()); - } else { - /* color change depending on velocity */ - std::unique_ptr dynamic_color_ptr = setColorDependsOnVelocity( - property_vel_max_->getFloat(), path_point.longitudinal_velocity_mps); - color = *dynamic_color_ptr; - } - color.a = property_velocity_alpha_->getFloat(); - - velocity_manual_object_->position( - path_point.pose.position.x, path_point.pose.position.y, - path_point.pose.position.z + - path_point.longitudinal_velocity_mps * property_velocity_scale_->getFloat()); - velocity_manual_object_->colour(color); - } - /* - * Velocity Text - */ - if (property_velocity_text_view_->getBool()) { - Ogre::Vector3 position; - position.x = path_point.pose.position.x; - position.y = path_point.pose.position.y; - position.z = path_point.pose.position.z; - Ogre::SceneNode * node = velocity_text_nodes_.at(point_idx); - node->setPosition(position); - - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - const double vel = path_point.longitudinal_velocity_mps; - std::stringstream ss; - ss << std::fixed << std::setprecision(2) << vel; - text->setCaption(ss.str()); - text->setCharacterHeight(property_velocity_text_scale_->getFloat()); - text->setVisible(true); - } else { - rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); - text->setVisible(false); - } - } - - path_manual_object_->end(); - velocity_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowareTrajectoryDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryDisplay, rviz_common::Display) diff --git a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp deleted file mode 100644 index b9c32aea58be3..0000000000000 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ /dev/null @@ -1,275 +0,0 @@ -// Copyright 2021 Tier IV, Inc. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include - -#define EIGEN_MPL2_ONLY -#include -#include -#include - -#include - -namespace rviz_plugins -{ -AutowareTrajectoryFootprintDisplay::AutowareTrajectoryFootprintDisplay() -{ - // trajectory footprint - property_trajectory_footprint_view_ = new rviz_common::properties::BoolProperty( - "View Trajectory Footprint", true, "", this, SLOT(updateVisualization()), this); - property_trajectory_footprint_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_trajectory_footprint_view_, SLOT(updateVisualization()), this); - property_trajectory_footprint_alpha_->setMin(0.0); - property_trajectory_footprint_alpha_->setMax(1.0); - property_trajectory_footprint_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(230, 230, 50), "", property_trajectory_footprint_view_, - SLOT(updateVisualization()), this); - property_vehicle_length_ = new rviz_common::properties::FloatProperty( - "Vehicle Length", 4.77, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_vehicle_width_ = new rviz_common::properties::FloatProperty( - "Vehicle Width", 1.83, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_rear_overhang_ = new rviz_common::properties::FloatProperty( - "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_offset_ = new rviz_common::properties::FloatProperty( - "Offset from BaseLink", 0.0, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), - this); - property_vehicle_length_->setMin(0.0); - property_vehicle_width_->setMin(0.0); - property_rear_overhang_->setMin(0.0); - - // trajectory point - property_trajectory_point_view_ = new rviz_common::properties::BoolProperty( - "View Trajectory Point", false, "", this, SLOT(updateVisualization()), this); - property_trajectory_point_alpha_ = new rviz_common::properties::FloatProperty( - "Alpha", 1.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - property_trajectory_point_alpha_->setMin(0.0); - property_trajectory_point_alpha_->setMax(1.0); - property_trajectory_point_color_ = new rviz_common::properties::ColorProperty( - "Color", QColor(0, 60, 255), "", property_trajectory_point_view_, SLOT(updateVisualization()), - this); - property_trajectory_point_radius_ = new rviz_common::properties::FloatProperty( - "Radius", 0.1, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - property_trajectory_point_offset_ = new rviz_common::properties::FloatProperty( - "Offset", 0.0, "", property_trajectory_point_view_, SLOT(updateVisualization()), this); - - updateVehicleInfo(); -} - -AutowareTrajectoryFootprintDisplay::~AutowareTrajectoryFootprintDisplay() -{ - if (initialized()) { - scene_manager_->destroyManualObject(trajectory_footprint_manual_object_); - scene_manager_->destroyManualObject(trajectory_point_manual_object_); - } -} - -void AutowareTrajectoryFootprintDisplay::onInitialize() -{ - MFDClass::onInitialize(); - - trajectory_footprint_manual_object_ = scene_manager_->createManualObject(); - trajectory_footprint_manual_object_->setDynamic(true); - scene_node_->attachObject(trajectory_footprint_manual_object_); - - trajectory_point_manual_object_ = scene_manager_->createManualObject(); - trajectory_point_manual_object_->setDynamic(true); - scene_node_->attachObject(trajectory_point_manual_object_); -} - -void AutowareTrajectoryFootprintDisplay::reset() -{ - MFDClass::reset(); - trajectory_footprint_manual_object_->clear(); - trajectory_point_manual_object_->clear(); -} - -bool AutowareTrajectoryFootprintDisplay::validateFloats( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr & msg_ptr) -{ - for (auto && trajectory_point : msg_ptr->points) { - if (!rviz_common::validateFloats(trajectory_point.pose)) { - return false; - } - } - return true; -} - -void AutowareTrajectoryFootprintDisplay::processMessage( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg_ptr) -{ - if (!validateFloats(msg_ptr)) { - setStatus( - rviz_common::properties::StatusProperty::Error, "Topic", - "Message contained invalid floating point values (nans or infs)"); - return; - } - - // This doesn't work in the constructor. - if (!vehicle_info_) { - try { - vehicle_info_ = std::make_shared( - VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); - updateVehicleInfo(); - } catch (const std::exception & e) { - RCLCPP_WARN_THROTTLE( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", - e.what()); - } - } - - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if (!context_->getFrameManager()->getTransform(msg_ptr->header, position, orientation)) { - RCLCPP_DEBUG( - rviz_ros_node_.lock()->get_raw_node()->get_logger(), - "Error transforming from frame '%s' to frame '%s'", msg_ptr->header.frame_id.c_str(), - qPrintable(fixed_frame_)); - } - - scene_node_->setPosition(position); - scene_node_->setOrientation(orientation); - - trajectory_footprint_manual_object_->clear(); - trajectory_point_manual_object_->clear(); - - Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( - "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); - material->setDepthWriteEnabled(false); - - if (!msg_ptr->points.empty()) { - trajectory_footprint_manual_object_->estimateVertexCount(msg_ptr->points.size() * 4 * 2); - trajectory_footprint_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); - - trajectory_point_manual_object_->estimateVertexCount(msg_ptr->points.size() * 3 * 8); - trajectory_point_manual_object_->begin( - "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); - - const float offset_from_baselink = property_offset_->getFloat(); - - for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { - const auto & path_point = msg_ptr->points.at(point_idx); - /* - * Footprint - */ - if (property_trajectory_footprint_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_trajectory_footprint_color_->getColor()); - color.a = property_trajectory_footprint_alpha_->getFloat(); - - const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang - offset_from_baselink; - const float bottom = -info->rear_overhang + offset_from_baselink; - const float left = -info->width / 2.0; - const float right = info->width / 2.0; - - const std::array lon_offset_vec{top, top, bottom, bottom}; - const std::array lat_offset_vec{left, right, right, left}; - - for (int f_idx = 0; f_idx < 4; ++f_idx) { - const Eigen::Quaternionf quat( - path_point.pose.orientation.w, path_point.pose.orientation.x, - path_point.pose.orientation.y, path_point.pose.orientation.z); - - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; - const auto offset_to_edge = quat * offset_vec; - trajectory_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - trajectory_footprint_manual_object_->colour(color); - } - { - const Eigen::Vector3f offset_vec{ - lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; - const auto offset_to_edge = quat * offset_vec; - trajectory_footprint_manual_object_->position( - path_point.pose.position.x + offset_to_edge.x(), - path_point.pose.position.y + offset_to_edge.y(), path_point.pose.position.z); - trajectory_footprint_manual_object_->colour(color); - } - } - } - - /* - * Point - */ - if (property_trajectory_point_view_->getBool()) { - Ogre::ColourValue color; - color = rviz_common::properties::qtToOgre(property_trajectory_point_color_->getColor()); - color.a = property_trajectory_point_alpha_->getFloat(); - - const double offset = property_trajectory_point_offset_->getFloat(); - const double yaw = tf2::getYaw(path_point.pose.orientation); - const double base_x = path_point.pose.position.x + offset * std::cos(yaw); - const double base_y = path_point.pose.position.y + offset * std::sin(yaw); - const double base_z = path_point.pose.position.z; - - const double radius = property_trajectory_point_radius_->getFloat(); - for (size_t s_idx = 0; s_idx < 8; ++s_idx) { - const double current_angle = static_cast(s_idx) / 8.0 * 2.0 * M_PI; - const double next_angle = static_cast(s_idx + 1) / 8.0 * 2.0 * M_PI; - trajectory_point_manual_object_->position( - base_x + radius * std::cos(current_angle), base_y + radius * std::sin(current_angle), - base_z); - trajectory_point_manual_object_->colour(color); - - trajectory_point_manual_object_->position( - base_x + radius * std::cos(next_angle), base_y + radius * std::sin(next_angle), base_z); - trajectory_point_manual_object_->colour(color); - - trajectory_point_manual_object_->position(base_x, base_y, base_z); - trajectory_point_manual_object_->colour(color); - } - } - } - - trajectory_footprint_manual_object_->end(); - trajectory_point_manual_object_->end(); - } - last_msg_ptr_ = msg_ptr; -} - -void AutowareTrajectoryFootprintDisplay::updateVisualization() -{ - if (last_msg_ptr_ != nullptr) { - processMessage(last_msg_ptr_); - } -} - -void AutowareTrajectoryFootprintDisplay::updateVehicleInfo() -{ - if (vehicle_info_) { - vehicle_footprint_info_ = std::make_shared( - vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, - vehicle_info_->rear_overhang_m); - } else { - const float length{property_vehicle_length_->getFloat()}; - const float width{property_vehicle_width_->getFloat()}; - const float rear_overhang{property_rear_overhang_->getFloat()}; - - vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); - } -} - -} // namespace rviz_plugins - -#include -PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareTrajectoryFootprintDisplay, rviz_common::Display) diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp index 0b9225bf94238..d90f2769b36d5 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.cpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -67,6 +68,8 @@ AutowareStatePanel::AutowareStatePanel(QWidget * parent) : rviz_common::Panel(pa h_layout->addWidget(makeFailSafeGroup()); v_layout->addLayout(h_layout); } + v_layout->addWidget(makeVelocityFactorsGroup()); + v_layout->addWidget(makeSteeringFactorsGroup()); v_layout->addLayout(gear_layout); velocity_limit_layout->addWidget(velocity_limit_button_ptr_); @@ -206,6 +209,51 @@ QGroupBox * AutowareStatePanel::makeFailSafeGroup() return group; } +QGroupBox * AutowareStatePanel::makeVelocityFactorsGroup() +{ + auto * group = new QGroupBox("VelocityFactors"); + auto * grid = new QGridLayout; + + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + + auto header_labels = QStringList({"Type", "Status", "Distance [m]", "Detail"}); + velocity_factors_table_ = new QTableWidget(); + velocity_factors_table_->setColumnCount(header_labels.size()); + velocity_factors_table_->setHorizontalHeaderLabels(header_labels); + velocity_factors_table_->setVerticalHeader(vertical_header); + velocity_factors_table_->setHorizontalHeader(horizontal_header); + grid->addWidget(velocity_factors_table_, 0, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareStatePanel::makeSteeringFactorsGroup() +{ + auto * group = new QGroupBox("SteeringFactors"); + auto * grid = new QGridLayout; + + auto vertical_header = new QHeaderView(Qt::Vertical); + vertical_header->hide(); + auto horizontal_header = new QHeaderView(Qt::Horizontal); + horizontal_header->setSectionResizeMode(QHeaderView::Stretch); + + auto header_labels = + QStringList({"Type", "Status", "Distance.1 [m]", "Distance.2 [m]", "Direction", "Detail"}); + steering_factors_table_ = new QTableWidget(); + steering_factors_table_->setColumnCount(header_labels.size()); + steering_factors_table_->setHorizontalHeaderLabels(header_labels); + steering_factors_table_->setVerticalHeader(vertical_header); + steering_factors_table_->setHorizontalHeader(horizontal_header); + grid->addWidget(steering_factors_table_, 1, 0); + + group->setLayout(grid); + return group; +} + void AutowareStatePanel::onInitialize() { raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); @@ -259,6 +307,15 @@ void AutowareStatePanel::onInitialize() "/api/fail_safe/mrm_state", rclcpp::QoS{1}.transient_local(), std::bind(&AutowareStatePanel::onMRMState, this, _1)); + // Planning + sub_velocity_factors_ = raw_node_->create_subscription( + "/api/planning/velocity_factors", 10, + std::bind(&AutowareStatePanel::onVelocityFactors, this, _1)); + + sub_steering_factors_ = raw_node_->create_subscription( + "/api/planning/steering_factors", 10, + std::bind(&AutowareStatePanel::onSteeringFactors, this, _1)); + // Others sub_gear_ = raw_node_->create_subscription( "/vehicle/status/gear_status", 10, std::bind(&AutowareStatePanel::onShift, this, _1)); @@ -513,6 +570,226 @@ void AutowareStatePanel::onMRMState(const MRMState::ConstSharedPtr msg) } } +void AutowareStatePanel::onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg) +{ + velocity_factors_table_->clearContents(); + velocity_factors_table_->setRowCount(msg->factors.size()); + + for (std::size_t i = 0; i < msg->factors.size(); i++) { + const auto & e = msg->factors.at(i); + + // type + { + auto label = new QLabel(); + switch (e.type) { + case VelocityFactor::SURROUNDING_OBSTACLE: + label->setText("SURROUNDING_OBSTACLE"); + break; + case VelocityFactor::ROUTE_OBSTACLE: + label->setText("ROUTE_OBSTACLE"); + break; + case VelocityFactor::INTERSECTION: + label->setText("INTERSECTION"); + break; + case VelocityFactor::CROSSWALK: + label->setText("CROSSWALK"); + break; + case VelocityFactor::REAR_CHECK: + label->setText("REAR_CHECK"); + break; + case VelocityFactor::USER_DEFINED_DETECTION_AREA: + label->setText("USER_DEFINED_DETECTION_AREA"); + break; + case VelocityFactor::NO_STOPPING_AREA: + label->setText("NO_STOPPING_AREA"); + break; + case VelocityFactor::STOP_SIGN: + label->setText("STOP_SIGN"); + break; + case VelocityFactor::TRAFFIC_SIGNAL: + label->setText("TRAFFIC_SIGNAL"); + break; + case VelocityFactor::V2I_GATE_CONTROL_ENTER: + label->setText("V2I_GATE_CONTROL_ENTER"); + break; + case VelocityFactor::V2I_GATE_CONTROL_LEAVE: + label->setText("V2I_GATE_CONTROL_LEAVE"); + break; + case VelocityFactor::MERGE: + label->setText("MERGE"); + break; + case VelocityFactor::SIDEWALK: + label->setText("SIDEWALK"); + break; + case VelocityFactor::LANE_CHANGE: + label->setText("LANE_CHANGE"); + break; + case VelocityFactor::AVOIDANCE: + label->setText("AVOIDANCE"); + break; + case VelocityFactor::EMERGENCY_STOP_OPERATION: + label->setText("EMERGENCY_STOP_OPERATION"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 0, label); + } + + // status + { + auto label = new QLabel(); + switch (e.status) { + case VelocityFactor::APPROACHING: + label->setText("APPROACHING"); + break; + case VelocityFactor::STOPPED: + label->setText("STOPPED"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 1, label); + } + + // distance + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance; + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 2, label); + } + + // detail + { + auto label = new QLabel(QString::fromStdString(e.detail)); + label->setAlignment(Qt::AlignCenter); + velocity_factors_table_->setCellWidget(i, 3, label); + } + } +} + +void AutowareStatePanel::onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg) +{ + steering_factors_table_->clearContents(); + steering_factors_table_->setRowCount(msg->factors.size()); + + for (std::size_t i = 0; i < msg->factors.size(); i++) { + const auto & e = msg->factors.at(i); + + // type + { + auto label = new QLabel(); + switch (e.type) { + case SteeringFactor::INTERSECTION: + label->setText("INTERSECTION"); + break; + case SteeringFactor::LANE_CHANGE: + label->setText("LANE_CHANGE"); + break; + case SteeringFactor::AVOIDANCE_PATH_CHANGE: + label->setText("AVOIDANCE_PATH_CHANGE"); + break; + case SteeringFactor::AVOIDANCE_PATH_RETURN: + label->setText("AVOIDANCE_PATH_RETURN"); + break; + case SteeringFactor::STATION: + label->setText("STATION"); + break; + case SteeringFactor::PULL_OUT: + label->setText("PULL_OUT"); + break; + case SteeringFactor::PULL_OVER: + label->setText("PULL_OVER"); + break; + case SteeringFactor::EMERGENCY_OPERATION: + label->setText("EMERGENCY_OPERATION"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 0, label); + } + + // status + { + auto label = new QLabel(); + switch (e.status) { + case SteeringFactor::APPROACHING: + label->setText("APPROACHING"); + break; + case SteeringFactor::TRYING: + label->setText("TRYING"); + break; + case SteeringFactor::TURNING: + label->setText("TURNING"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 1, label); + } + + // distance.1 + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance.front(); + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 2, label); + } + + // distance.2 + { + auto label = new QLabel(); + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << e.distance.back(); + label->setText(QString::fromStdString(ss.str())); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 3, label); + } + + // Direction + { + auto label = new QLabel(); + switch (e.direction) { + case SteeringFactor::LEFT: + label->setText("LEFT"); + break; + case SteeringFactor::RIGHT: + label->setText("RIGHT"); + break; + case SteeringFactor::STRAIGHT: + label->setText("STRAIGHT"); + break; + default: + label->setText("UNKNOWN"); + break; + } + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 4, label); + } + + // detail + { + auto label = new QLabel(QString::fromStdString(e.detail)); + label->setAlignment(Qt::AlignCenter); + steering_factors_table_->setCellWidget(i, 5, label); + } + } +} + void AutowareStatePanel::onShift( const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg) { diff --git a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp index b3e9b08152b16..52af59c241fe3 100644 --- a/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -30,6 +31,8 @@ #include #include #include +#include +#include #include #include #include @@ -53,6 +56,10 @@ class AutowareStatePanel : public rviz_common::Panel using MotionState = autoware_adapi_v1_msgs::msg::MotionState; using AcceptStart = autoware_adapi_v1_msgs::srv::AcceptStart; using MRMState = autoware_adapi_v1_msgs::msg::MrmState; + using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; + using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor; + using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; + using SteeringFactor = autoware_adapi_v1_msgs::msg::SteeringFactor; Q_OBJECT @@ -80,6 +87,8 @@ public Q_SLOTS: // NOLINT for Qt QGroupBox * makeLocalizationGroup(); QGroupBox * makeMotionGroup(); QGroupBox * makeFailSafeGroup(); + QGroupBox * makeVelocityFactorsGroup(); + QGroupBox * makeSteeringFactorsGroup(); void onShift(const autoware_auto_vehicle_msgs::msg::GearReport::ConstSharedPtr msg); void onEmergencyStatus(const tier4_external_api_msgs::msg::Emergency::ConstSharedPtr msg); @@ -150,6 +159,16 @@ public Q_SLOTS: // NOLINT for Qt void onMRMState(const MRMState::ConstSharedPtr msg); + // Planning + QTableWidget * velocity_factors_table_{nullptr}; + QTableWidget * steering_factors_table_{nullptr}; + + rclcpp::Subscription::SharedPtr sub_velocity_factors_; + rclcpp::Subscription::SharedPtr sub_steering_factors_; + + void onVelocityFactors(const VelocityFactorArray::ConstSharedPtr msg); + void onSteeringFactors(const SteeringFactorArray::ConstSharedPtr msg); + // Others QPushButton * velocity_limit_button_ptr_; QLabel * gear_label_ptr_; diff --git a/control/operation_mode_transition_manager/src/node.cpp b/control/operation_mode_transition_manager/src/node.cpp index d620e68ababe5..c54f9c1b76f43 100644 --- a/control/operation_mode_transition_manager/src/node.cpp +++ b/control/operation_mode_transition_manager/src/node.cpp @@ -51,12 +51,23 @@ OperationModeTransitionManager::OperationModeTransitionManager(const rclcpp::Nod } // initialize state - transition_timeout_ = declare_parameter("transition_timeout"); current_mode_ = OperationMode::STOP; transition_ = nullptr; gate_operation_mode_.mode = OperationModeState::UNKNOWN; gate_operation_mode_.is_in_transition = false; control_mode_report_.mode = ControlModeReport::NO_COMMAND; + transition_timeout_ = declare_parameter("transition_timeout"); + { + // check `transition_timeout` value + const auto stable_duration = declare_parameter("stable_check.duration"); + const double TIMEOUT_MARGIN = 0.5; + if (transition_timeout_ < stable_duration + TIMEOUT_MARGIN) { + transition_timeout_ = stable_duration + TIMEOUT_MARGIN; + RCLCPP_WARN( + get_logger(), "`transition_timeout` must be somewhat larger than `stable_check.duration`"); + RCLCPP_WARN_STREAM(get_logger(), "transition_timeout is set to " << transition_timeout_); + } + } // modes modes_[OperationMode::STOP] = std::make_unique(); diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index c823c11a4f4ba..f8434bbdb081f 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -71,7 +71,7 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) // params for mode change completed { auto & p = stable_check_param_; - p.duration = node->declare_parameter("stable_check.duration"); + p.duration = node->get_parameter("stable_check.duration").as_double(); p.dist_threshold = node->declare_parameter("stable_check.dist_threshold"); p.speed_upper_threshold = node->declare_parameter("stable_check.speed_upper_threshold"); p.speed_lower_threshold = node->declare_parameter("stable_check.speed_lower_threshold"); diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 95485a2706237..cf38c44cf562e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -113,6 +113,7 @@ + @@ -129,15 +130,6 @@ - - - - - - - - - @@ -241,25 +233,17 @@ - + - - - - - - - - - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index afc55582853f9..c072d4deb3059 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -249,7 +249,6 @@ def create_common_pipeline(self, input_topic, output_topic): return components def create_single_frame_obstacle_segmentation_components(self, input_topic, output_topic): - additional_lidars = self.ground_segmentation_param["additional_lidars"] use_ransac = bool(self.ground_segmentation_param["ransac_input_topics"]) use_additional = bool(additional_lidars) @@ -424,7 +423,6 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con @staticmethod def get_additional_lidars_concatenated_component(input_topics, output_topic): - return ComposableNode( package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", @@ -463,7 +461,9 @@ def launch_setup(context, *args, **kwargs): components.extend( pipeline.create_single_frame_obstacle_segmentation_components( input_topic=LaunchConfiguration("input/pointcloud"), - output_topic=pipeline.single_frame_obstacle_seg_output, + output_topic=pipeline.single_frame_obstacle_seg_output + if pipeline.use_single_frame_filter or pipeline.use_time_series_filter + else pipeline.output_topic, ) ) @@ -505,7 +505,6 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - launch_arguments = [] def add_launch_arg(name: str, default_value=None): diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index aa2d31ff15745..55650f5e6738c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -30,7 +30,6 @@ def launch_setup(context, *args, **kwargs): - # vehicle information parameter vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_param_path, "r") as f: diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index b6400b568837a..dca6866a6c811 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -28,7 +28,6 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -63,10 +62,7 @@ def launch_setup(context, *args, **kwargs): "vehicle_frame": "base_link", "map_frame": "map", "update_rate": 10.0, - "use_wayarea": True, - "use_parkinglot": True, - "use_objects": True, - "use_points": True, + "activate_by_scenario": True, "grid_min_value": 0.0, "grid_max_value": 1.0, "grid_resolution": 0.2, @@ -76,6 +72,10 @@ def launch_setup(context, *args, **kwargs): "grid_position_y": 0.0, "maximum_lidar_height_thres": 0.3, "minimum_lidar_height_thres": -2.2, + "use_wayarea": True, + "use_parkinglot": True, + "use_objects": True, + "use_points": True, "expand_polygon_size": 1.0, "size_of_expansion_kernel": 9, }, diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index 75d2c88bad060..6ae8cefc9088c 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -49,6 +49,7 @@ if(BUILD_TESTING) test/test_measurement.cpp test/test_numeric.cpp test/test_state_transition.cpp + test/test_string.cpp test/test_warning_message.cpp) foreach(filepath ${TEST_FILES}) diff --git a/localization/ekf_localizer/include/ekf_localizer/string.hpp b/localization/ekf_localizer/include/ekf_localizer/string.hpp new file mode 100644 index 0000000000000..a4cd1f320367c --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/string.hpp @@ -0,0 +1,29 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EKF_LOCALIZER__STRING_HPP_ +#define EKF_LOCALIZER__STRING_HPP_ + +#include + +inline std::string eraseLeadingSlash(const std::string & s) +{ + std::string a = s; + if (a.front() == '/') { + a.erase(0, 1); + } + return a; +} + +#endif // EKF_LOCALIZER__STRING_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/warning.hpp b/localization/ekf_localizer/include/ekf_localizer/warning.hpp index dcaae3c87b3b5..a3c8800242e2b 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning.hpp @@ -26,14 +26,14 @@ class Warning void warn(const std::string & message) const { - RCLCPP_WARN(node_->get_logger(), message.c_str()); + RCLCPP_WARN(node_->get_logger(), "%s", message.c_str()); } void warnThrottle(const std::string & message, const int duration_milliseconds) const { RCLCPP_WARN_THROTTLE( node_->get_logger(), *(node_->get_clock()), - std::chrono::milliseconds(duration_milliseconds).count(), message.c_str()); + std::chrono::milliseconds(duration_milliseconds).count(), "%s", message.c_str()); } private: diff --git a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp index dbe44df702af0..f6c664eb26451 100644 --- a/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/warning_message.hpp @@ -17,6 +17,10 @@ #include +std::string poseDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt); +std::string twistDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt); std::string poseDelayTimeWarningMessage(const double delay_time); std::string twistDelayTimeWarningMessage(const double delay_time); std::string mahalanobisWarningMessage(const double distance, const double max_distance); diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 5243015a64bae..a30ccef5a39bb 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -21,6 +21,7 @@ #include "ekf_localizer/numeric.hpp" #include "ekf_localizer/state_index.hpp" #include "ekf_localizer/state_transition.hpp" +#include "ekf_localizer/string.hpp" #include "ekf_localizer/warning.hpp" #include "ekf_localizer/warning_message.hpp" @@ -273,12 +274,9 @@ bool EKFLocalizer::getTransformFromTF( tf2::BufferCore tf_buffer; tf2_ros::TransformListener tf_listener(tf_buffer); rclcpp::sleep_for(std::chrono::milliseconds(100)); - if (parent_frame.front() == '/') { - parent_frame.erase(0, 1); - } - if (child_frame.front() == '/') { - child_frame.erase(0, 1); - } + + parent_frame = eraseLeadingSlash(parent_frame); + child_frame = eraseLeadingSlash(child_frame); for (int i = 0; i < 50; ++i) { try { @@ -404,13 +402,9 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > params_.extend_state_step - 1) { + if (delay_step >= params_.extend_state_step) { warning_.warnThrottle( - fmt::format( - "Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, params_.extend_state_step * ekf_dt_), - 1000); + poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -487,13 +481,9 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); - if (delay_step > params_.extend_state_step - 1) { + if (delay_step >= params_.extend_state_step) { warning_.warnThrottle( - fmt::format( - "Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = " - "extend_state_step * ekf_dt : %f [s]", - delay_time, params_.extend_state_step * ekf_dt_), - 1000); + twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); return; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); diff --git a/localization/ekf_localizer/src/state_transition.cpp b/localization/ekf_localizer/src/state_transition.cpp index 8c87436e60141..88fc9f76d7168 100644 --- a/localization/ekf_localizer/src/state_transition.cpp +++ b/localization/ekf_localizer/src/state_transition.cpp @@ -30,7 +30,7 @@ double normalizeYaw(const double & yaw) * y_{k+1} = y_k + vx_k * sin(yaw_k + b_k) * dt * yaw_{k+1} = yaw_k + (wz_k) * dt * b_{k+1} = b_k - * vx_{k+1} = vz_k + * vx_{k+1} = vx_k * wz_{k+1} = wz_k * * (b_k : yaw_bias_k) diff --git a/localization/ekf_localizer/src/warning_message.cpp b/localization/ekf_localizer/src/warning_message.cpp index 1088d5c061375..9a7bbf47a94eb 100644 --- a/localization/ekf_localizer/src/warning_message.cpp +++ b/localization/ekf_localizer/src/warning_message.cpp @@ -18,6 +18,24 @@ #include +std::string poseDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt) +{ + const std::string s = + "Pose delay exceeds the compensation limit, ignored. " + "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; + return fmt::format(s, delay_time, extend_state_step * ekf_dt); +} + +std::string twistDelayStepWarningMessage( + const double delay_time, const int extend_state_step, const double ekf_dt) +{ + const std::string s = + "Twist delay exceeds the compensation limit, ignored. " + "delay: {:.3f}[s], limit = extend_state_step * ekf_dt : {:.3f}[s]"; + return fmt::format(s, delay_time, extend_state_step * ekf_dt); +} + std::string poseDelayTimeWarningMessage(const double delay_time) { const std::string s = "Pose time stamp is inappropriate, set delay to 0[s]. delay = {:.3f}"; diff --git a/localization/ekf_localizer/test/test_string.cpp b/localization/ekf_localizer/test/test_string.cpp new file mode 100644 index 0000000000000..7b35a56d889e9 --- /dev/null +++ b/localization/ekf_localizer/test/test_string.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ekf_localizer/string.hpp" + +#include + +TEST(EraseLeadingSlash, SmokeTest) +{ + EXPECT_EQ(eraseLeadingSlash("/topic"), "topic"); + EXPECT_EQ(eraseLeadingSlash("topic"), "topic"); // do nothing + + EXPECT_EQ(eraseLeadingSlash(""), ""); + EXPECT_EQ(eraseLeadingSlash("/"), ""); +} diff --git a/localization/ekf_localizer/test/test_warning_message.cpp b/localization/ekf_localizer/test/test_warning_message.cpp index fc86df0d6cd80..b4a05afa844dc 100644 --- a/localization/ekf_localizer/test/test_warning_message.cpp +++ b/localization/ekf_localizer/test/test_warning_message.cpp @@ -14,8 +14,26 @@ #include "ekf_localizer/warning_message.hpp" +#include + #include +TEST(PoseDelayStepWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + poseDelayStepWarningMessage(6.0, 2, 2.0).c_str(), + "Pose delay exceeds the compensation limit, ignored. " + "delay: 6.000[s], limit = extend_state_step * ekf_dt : 4.000[s]"); +} + +TEST(TwistDelayStepWarningMessage, SmokeTest) +{ + EXPECT_STREQ( + twistDelayStepWarningMessage(10.0, 3, 2.0).c_str(), + "Twist delay exceeds the compensation limit, ignored. " + "delay: 10.000[s], limit = extend_state_step * ekf_dt : 6.000[s]"); +} + TEST(PoseDelayTimeWarningMessage, SmokeTest) { EXPECT_STREQ( diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 0f5b761baf6f0..0a04f1bb60466 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -34,6 +34,7 @@ ament_auto_add_executable(ndt_scan_matcher src/tf2_listener_module.cpp src/map_module.cpp src/pose_initialization_module.cpp + src/map_update_module.cpp ) link_directories(${PCL_LIBRARY_DIRS}) diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index 81fac59e4b8db..7c63785aebcd2 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -64,7 +64,6 @@ One optional function is regularization. Please see the regularization chapter i | `max_iterations` | int | The number of iterations required to calculate alignment | | `converged_param_type` | int | The type of indicators for scan matching score (0: TP, 1: NVTL) | | `converged_param_transform_probability` | double | Threshold for deciding whether to trust the estimation result | -| `neighborhood_search_method` | int | neighborhood search method (0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1) | | `num_threads` | int | Number of threads used for parallel computing | (TP: Transform Probability, NVTL: Nearest Voxel Transform Probability) @@ -172,6 +171,67 @@ The color of the trajectory indicates the error (meter) from the reference traje drawing drawing +## Dynamic map loading + +Autoware supports dynamic map loading feature for `ndt_scan_matcher`. Using this feature, NDT dynamically requests for the surrounding pointcloud map to `pointcloud_map_loader`, and then receive and preprocess the map in an online fashion. + +Using the feature, `ndt_scan_matcher` can theoretically handle any large size maps in terms of memory usage. (Note that it is still possible that there exists a limitation due to other factors, e.g. floating-point error) + +drawing + +### Additional interfaces + +#### Additional inputs + +| Name | Type | Description | +| ---------------- | ------------------------- | ----------------------------------------------------------- | +| `input_ekf_odom` | `nav_msgs::msg::Odometry` | Vehicle localization results (used for map update decision) | + +#### Additional outputs + +| Name | Type | Description | +| ----------------------------- | ------------------------------- | ------------------------------------------------- | +| `debug/loaded_pointcloud_map` | `sensor_msgs::msg::PointCloud2` | pointcloud maps used for localization (for debug) | + +#### Additional client + +| Name | Type | Description | +| ------------------- | ------------------------------------------------------ | ------------------ | +| `client_map_loader` | `autoware_map_msgs::srv::GetDifferentialPointCloudMap` | map loading client | + +### Parameters + +| Name | Type | Description | +| ------------------------------------- | ------ | -------------------------------------------------------------------- | +| `use_dynamic_map_loading` | bool | Flag to enable dynamic map loading feature for NDT (TRUE by default) | +| `dynamic_map_loading_update_distance` | double | Distance traveled to load new map(s) | +| `dynamic_map_loading_map_radius` | double | Map loading radius for every update | +| `lidar_radius` | double | LiDAR radius used for localization (only used for diagnosis) | + +### Enabling the dynamic map loading feature + +To use dynamic map loading feature for `ndt_scan_matcher`, you also need to appropriately configure some other settings outside of this node. +Follow the next two instructions. + +1. enable dynamic map loading interface in `pointcloud_map_loader` (by setting `enable_differential_load` to true in the package) +2. split the PCD files into grids (recommended size: 20[m] x 20[m]) + +Note that the dynamic map loading may FAIL if the map is split into two or more large size map (e.g. 1000[m] x 1000[m]). Please provide either of + +- one PCD map file +- multiple PCD map files divided into small size (~20[m]) + +Here is a split PCD map for `sample-map-rosbag` from Autoware tutorial: [`sample-map-rosbag_split.zip`](https://github.com/autowarefoundation/autoware.universe/files/10349104/sample-map-rosbag_split.zip) + +| PCD files | `use_dynamic_map_loading` | `enable_differential_load` | How NDT loads map(s) | +| :---------: | :-----------------------: | :------------------------: | :------------------: | +| single file | true | true | at once (standard) | +| single file | true | false | **does NOT work** | +| single file | false | true/false | at once (standard) | +| splitted | true | true | dynamically | +| splitted | true | false | **does NOT work** | +| splitted | false | true/false | at once (standard) | + ## Scan matching score based on de-grounded LiDAR scan ### Abstract diff --git a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml index 1fb8fd003b4ac..56c5baa347aaa 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -1,5 +1,7 @@ /**: ros__parameters: + # Use dynamic map loading + use_dynamic_map_loading: true # Vehicle reference frame base_frame: "base_link" @@ -41,10 +43,6 @@ # Tolerance of distance difference between two initial poses used for linear interpolation. [m] initial_pose_distance_tolerance_m: 10.0 - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - # Number of threads used for parallel computing num_threads: 4 @@ -63,9 +61,18 @@ # Regularization switch regularization_enabled: false - # Regularization scale factor + # regularization scale factor regularization_scale_factor: 0.01 + # Dynamic map loading distance + dynamic_map_loading_update_distance: 20.0 + + # Dynamic map loading loading radius + dynamic_map_loading_map_radius: 150.0 + + # Radius of input LiDAR range (used for diagnostics of dynamic map loading) + lidar_radius: 100.0 + # A flag for using scan matching score based on de-grounded LiDAR scan estimate_scores_for_degrounded_scan: false diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp index 871ff2760acf7..5253879a28937 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_module.hpp @@ -19,9 +19,9 @@ #include +#include #include #include -#include #include @@ -30,7 +30,7 @@ class MapModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: MapModule( diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp new file mode 100644 index 0000000000000..fb9577ca42934 --- /dev/null +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -0,0 +1,107 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ +#define NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ + +#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" +#include "ndt_scan_matcher/tf2_listener_module.hpp" +#include "ndt_scan_matcher/util_func.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +class MapUpdateModule +{ + using PointSource = pcl::PointXYZ; + using PointTarget = pcl::PointXYZ; + using NormalDistributionsTransform = + pclomp::MultiGridNormalDistributionsTransform; + +public: + MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr); + +private: + void service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res); + void callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr); + void map_update_timer_callback(); + + void update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove); + void update_map(const geometry_msgs::msg::Point & position); + bool should_update_map(const geometry_msgs::msg::Point & position) const; + void publish_partial_pcd_map(); + geometry_msgs::msg::PoseWithCovarianceStamped align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov); + void publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr); + + rclcpp::Publisher::SharedPtr loaded_pcd_pub_; + rclcpp::Publisher::SharedPtr + ndt_monte_carlo_initial_pose_marker_pub_; + rclcpp::Publisher::SharedPtr sensor_aligned_pose_pub_; + + rclcpp::Service::SharedPtr service_; + rclcpp::Client::SharedPtr + pcd_loader_client_; + rclcpp::TimerBase::SharedPtr map_update_timer_; + + rclcpp::Subscription::SharedPtr ekf_odom_sub_; + + rclcpp::CallbackGroup::SharedPtr map_callback_group_; + + std::shared_ptr ndt_ptr_; + std::mutex * ndt_ptr_mutex_; + std::string map_frame_; + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + std::shared_ptr tf2_listener_module_; + std::shared_ptr> state_ptr_; + + int initial_estimate_particles_num_; + std::optional last_update_position_ = std::nullopt; + std::optional current_position_ = std::nullopt; + const double dynamic_map_loading_update_distance_; + const double dynamic_map_loading_map_radius_; + const double lidar_radius_; +}; + +#endif // NDT_SCAN_MATCHER__MAP_UPDATE_MODULE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 30356bc61cee0..1bd7a509a3a7b 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -18,6 +18,7 @@ #define FMT_HEADER_ONLY #include "ndt_scan_matcher/map_module.hpp" +#include "ndt_scan_matcher/map_update_module.hpp" #include "ndt_scan_matcher/pose_initialization_module.hpp" #include "ndt_scan_matcher/tf2_listener_module.hpp" @@ -34,8 +35,8 @@ #include #include +#include #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -71,7 +72,7 @@ class NDTScanMatcher : public rclcpp::Node using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: NDTScanMatcher(); @@ -200,6 +201,7 @@ class NDTScanMatcher : public rclcpp::Node std::shared_ptr tf2_listener_module_; std::unique_ptr map_module_; std::unique_ptr pose_init_module_; + std::unique_ptr map_update_module_; bool estimate_scores_for_degrounded_scan_; double z_margin_for_ground_removal_; diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp index 7745ecf8a2148..0e1a6e6816413 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/pose_initialization_module.hpp @@ -27,8 +27,8 @@ #include #include +#include #include -#include #include #include @@ -39,7 +39,7 @@ class PoseInitializationModule using PointSource = pcl::PointXYZ; using PointTarget = pcl::PointXYZ; using NormalDistributionsTransform = - pclomp::NormalDistributionsTransform; + pclomp::MultiGridNormalDistributionsTransform; public: PoseInitializationModule( diff --git a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml index 9e3d966bd770a..b442ac1b3d843 100644 --- a/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml +++ b/localization/ndt_scan_matcher/launch/ndt_scan_matcher.launch.xml @@ -7,10 +7,13 @@ + + + @@ -23,6 +26,9 @@ + + + diff --git a/localization/ndt_scan_matcher/media/differential_area_loading.gif b/localization/ndt_scan_matcher/media/differential_area_loading.gif new file mode 100644 index 0000000000000..4283b738a1dfa Binary files /dev/null and b/localization/ndt_scan_matcher/media/differential_area_loading.gif differ diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index 13b0f186ceb98..581879322caf3 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -14,6 +14,7 @@ autoware_cmake + autoware_map_msgs diagnostic_msgs fmt geometry_msgs diff --git a/localization/ndt_scan_matcher/src/map_update_module.cpp b/localization/ndt_scan_matcher/src/map_update_module.cpp new file mode 100644 index 0000000000000..2e8052b41d413 --- /dev/null +++ b/localization/ndt_scan_matcher/src/map_update_module.cpp @@ -0,0 +1,299 @@ +// Copyright 2022 Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "ndt_scan_matcher/map_update_module.hpp" + +template +double norm_xy(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +MapUpdateModule::MapUpdateModule( + rclcpp::Node * node, std::mutex * ndt_ptr_mutex, + std::shared_ptr ndt_ptr, + std::shared_ptr tf2_listener_module, std::string map_frame, + rclcpp::CallbackGroup::SharedPtr main_callback_group, + std::shared_ptr> state_ptr) +: ndt_ptr_(ndt_ptr), + ndt_ptr_mutex_(ndt_ptr_mutex), + map_frame_(map_frame), + logger_(node->get_logger()), + clock_(node->get_clock()), + tf2_listener_module_(tf2_listener_module), + state_ptr_(state_ptr), + dynamic_map_loading_update_distance_( + node->declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_( + node->declare_parameter("dynamic_map_loading_map_radius")), + lidar_radius_(node->declare_parameter("lidar_radius")) +{ + initial_estimate_particles_num_ = node->declare_parameter("initial_estimate_particles_num"); + + sensor_aligned_pose_pub_ = + node->create_publisher("monte_carlo_points_aligned", 10); + ndt_monte_carlo_initial_pose_marker_pub_ = + node->create_publisher( + "monte_carlo_initial_pose_marker", 10); + + auto main_sub_opt = rclcpp::SubscriptionOptions(); + main_sub_opt.callback_group = main_callback_group; + + map_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + ekf_odom_sub_ = node->create_subscription( + "ekf_odom", 100, std::bind(&MapUpdateModule::callback_ekf_odom, this, std::placeholders::_1), + main_sub_opt); + + loaded_pcd_pub_ = node->create_publisher( + "debug/loaded_pointcloud_map", rclcpp::QoS{1}.transient_local()); + + service_ = node->create_service( + "ndt_align_srv", + std::bind( + &MapUpdateModule::service_ndt_align, this, std::placeholders::_1, std::placeholders::_2), + rclcpp::ServicesQoS().get_rmw_qos_profile(), map_callback_group_); + + pcd_loader_client_ = node->create_client( + "pcd_loader_service", rmw_qos_profile_services_default); + while (!pcd_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO( + logger_, + "Waiting for pcd loader service. Check if the enable_differential_load in " + "pointcloud_map_loader is set `true`."); + } + + double map_update_dt = 1.0; + auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + node, clock_, period_ns, std::bind(&MapUpdateModule::map_update_timer_callback, this), + map_callback_group_); +} + +void MapUpdateModule::service_ndt_align( + const tier4_localization_msgs::srv::PoseWithCovarianceStamped::Request::SharedPtr req, + tier4_localization_msgs::srv::PoseWithCovarianceStamped::Response::SharedPtr res) +{ + // get TF from pose_frame to map_frame + auto TF_pose_to_map_ptr = std::make_shared(); + tf2_listener_module_->get_transform( + clock_->now(), map_frame_, req->pose_with_covariance.header.frame_id, TF_pose_to_map_ptr); + + // transform pose_frame to map_frame + const auto mapTF_initial_pose_msg = transform(req->pose_with_covariance, *TF_pose_to_map_ptr); + update_map(mapTF_initial_pose_msg.pose.pose.position); + + if (ndt_ptr_->getInputTarget() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputTarget"); + return; + } + + if (ndt_ptr_->getInputSource() == nullptr) { + res->success = false; + RCLCPP_WARN(logger_, "No InputSource"); + return; + } + + // mutex Map + std::lock_guard lock(*ndt_ptr_mutex_); + + (*state_ptr_)["state"] = "Aligning"; + res->pose_with_covariance = align_using_monte_carlo(ndt_ptr_, mapTF_initial_pose_msg); + (*state_ptr_)["state"] = "Sleeping"; + res->success = true; + res->pose_with_covariance.pose.covariance = req->pose_with_covariance.pose.covariance; + + last_update_position_ = res->pose_with_covariance.pose.pose.position; +} + +void MapUpdateModule::callback_ekf_odom(nav_msgs::msg::Odometry::ConstSharedPtr odom_ptr) +{ + current_position_ = odom_ptr->pose.pose.position; + + if (last_update_position_ == std::nullopt) { + return; + } + double distance = norm_xy(current_position_.value(), last_update_position_.value()); + if (distance + lidar_radius_ > dynamic_map_loading_map_radius_) { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 1, "Dynamic map loading is not keeping up."); + } +} + +void MapUpdateModule::map_update_timer_callback() +{ + if (current_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 1, + "Cannot find the reference position for map update. Please check if the EKF odometry is " + "provided to NDT."); + return; + } + if (last_update_position_ == std::nullopt) return; + + // continue only if we should update the map + if (should_update_map(current_position_.value())) { + RCLCPP_INFO(logger_, "Start updating NDT map (timer_callback)"); + update_map(current_position_.value()); + last_update_position_ = current_position_; + } +} + +bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & position) const +{ + if (last_update_position_ == std::nullopt) return false; + double distance = norm_xy(position, last_update_position_.value()); + return distance > dynamic_map_loading_update_distance_; +} + +void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position) +{ + auto request = std::make_shared(); + request->area.center = position; + request->area.radius = dynamic_map_loading_map_radius_; + request->cached_ids = ndt_ptr_->getCurrentMapIDs(); + + // // send a request to map_loader + auto result{pcd_loader_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "waiting response"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + update_ndt(result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); +} + +void MapUpdateModule::update_ndt( + const std::vector & maps_to_add, + const std::vector & map_ids_to_remove) +{ + RCLCPP_INFO( + logger_, "Update map (Add: %lu, Remove: %lu)", maps_to_add.size(), map_ids_to_remove.size()); + if (maps_to_add.empty() && map_ids_to_remove.empty()) { + RCLCPP_INFO(logger_, "Skip map update"); + return; + } + const auto exe_start_time = std::chrono::system_clock::now(); + + NormalDistributionsTransform backup_ndt = *ndt_ptr_; + + // Add pcd + for (const auto & map_to_add : maps_to_add) { + pcl::shared_ptr> map_points_ptr(new pcl::PointCloud); + pcl::fromROSMsg(map_to_add.pointcloud, *map_points_ptr); + backup_ndt.addTarget(map_points_ptr, map_to_add.cell_id); + } + + // Remove pcd + for (const std::string & map_id_to_remove : map_ids_to_remove) { + backup_ndt.removeTarget(map_id_to_remove); + } + + backup_ndt.createVoxelKdtree(); + + const auto exe_end_time = std::chrono::system_clock::now(); + const double exe_time = + std::chrono::duration_cast(exe_end_time - exe_start_time).count() / + 1000.0; + RCLCPP_INFO(logger_, "Time duration for creating new ndt_ptr: %lf [ms]", exe_time); + + // swap + (*ndt_ptr_mutex_).lock(); + // ToDo (kminoda): Here negligible NDT copy occurs during the new map loading phase, which should + // ideally be avoided. But I will leave this for now since I cannot come up with a solution other + // than using pointer of pointer. + *ndt_ptr_ = backup_ndt; + (*ndt_ptr_mutex_).unlock(); + + publish_partial_pcd_map(); +} + +geometry_msgs::msg::PoseWithCovarianceStamped MapUpdateModule::align_using_monte_carlo( + const std::shared_ptr & ndt_ptr, + const geometry_msgs::msg::PoseWithCovarianceStamped & initial_pose_with_cov) +{ + if (ndt_ptr->getInputTarget() == nullptr || ndt_ptr->getInputSource() == nullptr) { + RCLCPP_WARN(logger_, "No Map or Sensor PointCloud"); + return geometry_msgs::msg::PoseWithCovarianceStamped(); + } + + // generateParticle + const auto initial_poses = + create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); + + std::vector particle_array; + auto output_cloud = std::make_shared>(); + + for (unsigned int i = 0; i < initial_poses.size(); i++) { + const auto & initial_pose = initial_poses[i]; + const Eigen::Matrix4f initial_pose_matrix = pose_to_matrix4f(initial_pose); + ndt_ptr->align(*output_cloud, initial_pose_matrix); + const pclomp::NdtResult ndt_result = ndt_ptr->getResult(); + + Particle particle( + initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, + ndt_result.iteration_num); + particle_array.push_back(particle); + const auto marker_array = make_debug_markers( + clock_->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), particle, + i); + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + + auto sensor_points_mapTF_ptr = std::make_shared>(); + pcl::transformPointCloud(*ndt_ptr->getInputSource(), *sensor_points_mapTF_ptr, ndt_result.pose); + publish_point_cloud(initial_pose_with_cov.header.stamp, map_frame_, sensor_points_mapTF_ptr); + } + + auto best_particle_ptr = std::max_element( + std::begin(particle_array), std::end(particle_array), + [](const Particle & lhs, const Particle & rhs) { return lhs.score < rhs.score; }); + + geometry_msgs::msg::PoseWithCovarianceStamped result_pose_with_cov_msg; + result_pose_with_cov_msg.header.stamp = initial_pose_with_cov.header.stamp; + result_pose_with_cov_msg.header.frame_id = map_frame_; + result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + + return result_pose_with_cov_msg; +} + +void MapUpdateModule::publish_point_cloud( + const rclcpp::Time & sensor_ros_time, const std::string & frame_id, + const std::shared_ptr> & sensor_points_mapTF_ptr) +{ + sensor_msgs::msg::PointCloud2 sensor_points_mapTF_msg; + pcl::toROSMsg(*sensor_points_mapTF_ptr, sensor_points_mapTF_msg); + sensor_points_mapTF_msg.header.stamp = sensor_ros_time; + sensor_points_mapTF_msg.header.frame_id = frame_id; + sensor_aligned_pose_pub_->publish(sensor_points_mapTF_msg); +} + +void MapUpdateModule::publish_partial_pcd_map() +{ + pcl::PointCloud map_pcl = ndt_ptr_->getVoxelPCD(); + + sensor_msgs::msg::PointCloud2 map_msg; + pcl::toROSMsg(map_pcl, map_msg); + map_msg.header.frame_id = "map"; + + loaded_pcd_pub_->publish(map_msg); +} diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 877ffef7908dc..a9c37d7843a64 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -116,8 +116,6 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); ndt_params.max_iterations = this->declare_parameter("max_iterations"); - int search_method = this->declare_parameter("neighborhood_search_method"); - ndt_params.search_method = static_cast(search_method); ndt_params.num_threads = this->declare_parameter("num_threads"); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = @@ -222,10 +220,17 @@ NDTScanMatcher::NDTScanMatcher() diagnostic_thread_.detach(); tf2_listener_module_ = std::make_shared(this); - map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); - pose_init_module_ = std::make_unique( - this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, - state_ptr_); + + if (this->declare_parameter("use_dynamic_map_loading")) { + map_update_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } else { + map_module_ = std::make_unique(this, &ndt_ptr_mtx_, ndt_ptr_, main_callback_group); + pose_init_module_ = std::make_unique( + this, &ndt_ptr_mtx_, ndt_ptr_, tf2_listener_module_, map_frame_, main_callback_group, + state_ptr_); + } } void NDTScanMatcher::timer_diagnostic() diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml index 8f3ccbff00360..61b02c490c663 100644 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -2,8 +2,8 @@ ros__parameters: enable_whole_load: true enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false + enable_partial_load: true + enable_differential_load: true # only used when downsample_whole_load enabled leaf_size: 3.0 # downsample leaf size [m] diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml index ed9c691e86e30..08457362b23bb 100644 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ b/map/map_loader/launch/lanelet2_map_loader.launch.xml @@ -3,7 +3,6 @@ - @@ -11,10 +10,7 @@ - - - diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py index 75abe2164ca04..24758a46f75aa 100644 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/map_loader/test/lanelet2_map_loader_launch.test.py @@ -27,7 +27,6 @@ @pytest.mark.launch_test def generate_test_description(): - lanelet2_map_path = os.path.join( get_package_share_directory("map_loader"), "test/data/test_map.osm" ) diff --git a/mkdocs.yaml b/mkdocs.yaml index cc2b7513e7470..d448ab04c9a41 100644 --- a/mkdocs.yaml +++ b/mkdocs.yaml @@ -15,6 +15,7 @@ theme: - navigation.tabs - navigation.tabs.sticky - navigation.top + - navigation.footer favicon: docs/assets/images/autoware-foundation.png icon: logo: fontawesome/solid/car diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp index 165b9f9929894..577e0ff12c367 100644 --- a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp +++ b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp @@ -44,6 +44,7 @@ #include #include +#include #include #include @@ -78,9 +79,13 @@ class DetectionByTracker : public rclcpp::Node std::shared_ptr shape_estimator_; std::shared_ptr cluster_; std::shared_ptr debugger_; + std::map max_search_distance_for_merger_; + std::map max_search_distance_for_divider_; bool ignore_unknown_tracker_; + void setMaxSearchRange(); + void onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg); diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp index 226aba129da21..b9ee6fa9b8b6f 100644 --- a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp +++ b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp @@ -159,12 +159,39 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options) ignore_unknown_tracker_ = declare_parameter("ignore_unknown_tracker", true); + // set maximum search setting for merger/divider + setMaxSearchRange(); + shape_estimator_ = std::make_shared(true, true); cluster_ = std::make_shared( false, 10, 10000, 0.7, 0.3, 0); debugger_ = std::make_shared(this); } +void DetectionByTracker::setMaxSearchRange() +{ + using Label = autoware_auto_perception_msgs::msg::ObjectClassification; + // set max search distance for merger + max_search_distance_for_merger_[Label::UNKNOWN] = 5.0; + max_search_distance_for_merger_[Label::CAR] = 5.0; + max_search_distance_for_merger_[Label::TRUCK] = 8.0; + max_search_distance_for_merger_[Label::BUS] = 8.0; + max_search_distance_for_merger_[Label::TRAILER] = 10.0; + max_search_distance_for_merger_[Label::MOTORCYCLE] = 2.0; + max_search_distance_for_merger_[Label::BICYCLE] = 1.0; + max_search_distance_for_merger_[Label::PEDESTRIAN] = 1.0; + + // set max search distance for divider + max_search_distance_for_divider_[Label::UNKNOWN] = 6.0; + max_search_distance_for_divider_[Label::CAR] = 6.0; + max_search_distance_for_divider_[Label::TRUCK] = 9.0; + max_search_distance_for_divider_[Label::BUS] = 9.0; + max_search_distance_for_divider_[Label::TRAILER] = 11.0; + max_search_distance_for_divider_[Label::MOTORCYCLE] = 3.0; + max_search_distance_for_divider_[Label::BICYCLE] = 2.0; + max_search_distance_for_divider_[Label::PEDESTRIAN] = 2.0; +} + void DetectionByTracker::onObjects( const tier4_perception_msgs::msg::DetectedObjectsWithFeature::ConstSharedPtr input_msg) { @@ -222,7 +249,6 @@ void DetectionByTracker::divideUnderSegmentedObjects( { constexpr float recall_min_threshold = 0.4; constexpr float precision_max_threshold = 0.5; - constexpr float max_search_range = 6.0; constexpr float min_score_threshold = 0.4; out_objects.header = in_cluster_objects.header; @@ -232,6 +258,9 @@ void DetectionByTracker::divideUnderSegmentedObjects( const auto & label = tracked_object.classification.front().label; if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + // change search range according to label type + const float max_search_range = max_search_distance_for_divider_[label]; + std::optional highest_score_divided_object = std::nullopt; float highest_score = 0.0; @@ -357,7 +386,6 @@ void DetectionByTracker::mergeOverSegmentedObjects( tier4_perception_msgs::msg::DetectedObjectsWithFeature & out_objects) { constexpr float precision_threshold = 0.5; - constexpr float max_search_range = 5.0; out_objects.header = in_cluster_objects.header; out_no_found_tracked_objects.header = tracked_objects.header; @@ -365,6 +393,9 @@ void DetectionByTracker::mergeOverSegmentedObjects( const auto & label = tracked_object.classification.front().label; if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue; + // change search range according to label type + const float max_search_range = max_search_distance_for_merger_[label]; + // extend shape autoware_auto_perception_msgs::msg::DetectedObject extended_tracked_object = tracked_object; extended_tracked_object.shape = extendShape(tracked_object.shape, /*scale*/ 1.1); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 303d2acd9801f..5d6aba9b7d51d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -44,7 +44,9 @@ class RoiClusterFusionNode bool use_cluster_semantic_type_{false}; float iou_threshold_{0.0f}; bool remove_unknown_; + float trust_distance_; + bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 0f4327bb35605..2848a16c33af1 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -20,6 +20,7 @@ + @@ -75,6 +76,7 @@ + diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index fc3e714e8c91f..717f21da21fc3 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -40,6 +40,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); iou_threshold_ = declare_parameter("iou_threshold", 0.1); remove_unknown_ = declare_parameter("remove_unknown", false); + trust_distance_ = declare_parameter("trust_distance", 100.0); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -103,6 +104,10 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } + if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + continue; + } + // filter point out of scope if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { continue; @@ -225,6 +230,13 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } +bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +{ + const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; + const auto square_distance = position.x * position.x + position.y + position.y; + return square_distance > trust_distance_ * trust_distance_; +} + } // namespace image_projection_based_fusion #include diff --git a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py index f657270865c7e..0a9adafc3f641 100644 --- a/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py +++ b/perception/lidar_apollo_segmentation_tvm_nodes/test/launch.test.py @@ -24,7 +24,6 @@ @pytest.mark.launch_test def generate_test_description(): - lidar_apollo_segmentation_tvm = Node( package="lidar_apollo_segmentation_tvm_nodes", executable="lidar_apollo_segmentation_tvm_nodes_exe", diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 602b81ae5b946..970f69921c6e2 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -16,7 +16,7 @@ autoware_auto_perception_msgs pcl_ros perception_utils - python3-open3d-pip + python3-open3d rclcpp rclcpp_components tf2_eigen diff --git a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py index 3153351922219..5a1135379615f 100755 --- a/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py +++ b/perception/lidar_centerpoint/scripts/lidar_centerpoint_visualizer.py @@ -22,7 +22,6 @@ def main(args=None): - rclpy.init(args=args) node = Node("lidar_centerpoint_visualizer") @@ -53,5 +52,4 @@ def main(args=None): if __name__ == "__main__": - main() diff --git a/perception/shape_estimation/CMakeLists.txt b/perception/shape_estimation/CMakeLists.txt index 139c9e6d28f67..5ae6002cd7c3b 100644 --- a/perception/shape_estimation/CMakeLists.txt +++ b/perception/shape_estimation/CMakeLists.txt @@ -24,6 +24,7 @@ ament_auto_add_library(shape_estimation_lib SHARED lib/filter/car_filter.cpp lib/filter/bus_filter.cpp lib/filter/truck_filter.cpp + lib/filter/trailer_filter.cpp lib/filter/no_filter.cpp lib/filter/utils.cpp lib/corrector/no_corrector.cpp diff --git a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp index 73e6770c32170..d52bdc21f916f 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/corrector.hpp @@ -20,6 +20,7 @@ #include "shape_estimation/corrector/corrector_interface.hpp" #include "shape_estimation/corrector/no_corrector.hpp" #include "shape_estimation/corrector/reference_shape_size_corrector.hpp" +#include "shape_estimation/corrector/trailer_corrector.hpp" #include "shape_estimation/corrector/truck_corrector.hpp" #endif // SHAPE_ESTIMATION__CORRECTOR__CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp new file mode 100644 index 0000000000000..6dd885353c78f --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/corrector/trailer_corrector.hpp @@ -0,0 +1,43 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ +#define SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ + +#include "shape_estimation/corrector/vehicle_corrector.hpp" +#include "utils.hpp" + +// Generally speaking, trailer would be much larger than bus and truck. +// But currently we do not make large differences among bus/truck/trailer +// because current our vehicle classification is not reliable enough. +class TrailerCorrector : public VehicleCorrector +{ +public: + explicit TrailerCorrector(const bool use_reference_yaw = false) + : VehicleCorrector(use_reference_yaw) + { + corrector_utils::CorrectionBBParameters params; + params.min_width = 2.0; + params.max_width = 3.2; + params.default_width = (params.min_width + params.max_width) * 0.5; + params.min_length = 5.0; + params.max_length = 24.0; + params.default_length = 8.0; // Ideally, it should be 12m from average Japanese trailer size. + setParams(params); + } + + ~TrailerCorrector() = default; +}; + +#endif // SHAPE_ESTIMATION__CORRECTOR__TRAILER_CORRECTOR_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp index cfbfd39a08dcc..b205cbd6791ba 100644 --- a/perception/shape_estimation/include/shape_estimation/filter/filter.hpp +++ b/perception/shape_estimation/include/shape_estimation/filter/filter.hpp @@ -19,6 +19,7 @@ #include "shape_estimation/filter/car_filter.hpp" #include "shape_estimation/filter/filter_interface.hpp" #include "shape_estimation/filter/no_filter.hpp" +#include "shape_estimation/filter/trailer_filter.hpp" #include "shape_estimation/filter/truck_filter.hpp" #endif // SHAPE_ESTIMATION__FILTER__FILTER_HPP_ diff --git a/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp b/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp new file mode 100644 index 0000000000000..83c9ab0d8a944 --- /dev/null +++ b/perception/shape_estimation/include/shape_estimation/filter/trailer_filter.hpp @@ -0,0 +1,33 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ +#define SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ + +#include "shape_estimation/filter/filter_interface.hpp" +#include "utils.hpp" + +class TrailerFilter : public ShapeEstimationFilterInterface +{ +public: + TrailerFilter() = default; + + ~TrailerFilter() = default; + + bool filter( + const autoware_auto_perception_msgs::msg::Shape & shape, + const geometry_msgs::msg::Pose & pose) override; +}; + +#endif // SHAPE_ESTIMATION__FILTER__TRAILER_FILTER_HPP_ diff --git a/perception/shape_estimation/lib/filter/trailer_filter.cpp b/perception/shape_estimation/lib/filter/trailer_filter.cpp new file mode 100644 index 0000000000000..368985d5303db --- /dev/null +++ b/perception/shape_estimation/lib/filter/trailer_filter.cpp @@ -0,0 +1,25 @@ +// Copyright 2018 Autoware Foundation. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "shape_estimation/filter/trailer_filter.hpp" + +bool TrailerFilter::filter( + const autoware_auto_perception_msgs::msg::Shape & shape, + [[maybe_unused]] const geometry_msgs::msg::Pose & pose) +{ + constexpr float min_width = 1.5; + constexpr float max_width = 3.2; + constexpr float max_length = 25.0; // maximum Full-TRAILER size in JAPAN(normally upto 18m) + return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); +} diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index 12dd066c2ca41..672082fe305fd 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -20,6 +20,6 @@ bool TruckFilter::filter( { constexpr float min_width = 1.5; constexpr float max_width = 3.2; - constexpr float max_length = 7.9; + constexpr float max_length = 7.9; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index ac2024e4787cc..9577b1e2d3197 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -92,8 +92,10 @@ bool ShapeEstimator::applyFilter( filter_ptr.reset(new CarFilter); } else if (label == Label::BUS) { filter_ptr.reset(new BusFilter); - } else if (label == Label::TRUCK || label == Label::TRAILER) { + } else if (label == Label::TRUCK) { filter_ptr.reset(new TruckFilter); + } else if (label == Label::TRAILER) { + filter_ptr.reset(new TrailerFilter); } else { filter_ptr.reset(new NoFilter); } @@ -114,8 +116,10 @@ bool ShapeEstimator::applyCorrector( corrector_ptr.reset(new CarCorrector(use_reference_yaw)); } else if (label == Label::BUS) { corrector_ptr.reset(new BusCorrector(use_reference_yaw)); - } else if (label == Label::TRUCK || label == Label::TRAILER) { + } else if (label == Label::TRUCK) { corrector_ptr.reset(new TruckCorrector(use_reference_yaw)); + } else if (label == Label::TRAILER) { + corrector_ptr.reset(new TrailerCorrector(use_reference_yaw)); } else { corrector_ptr.reset(new NoCorrector); } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 07d5621ec7fe9..b1e1c110782fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -143,7 +143,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node PullOutParameters getPullOutParam(); // callback - void onOdometry(const Odometry::SharedPtr msg); + void onOdometry(const Odometry::ConstSharedPtr msg); void onAcceleration(const AccelWithCovarianceStamped::ConstSharedPtr msg); void onPerception(const PredictedObjects::ConstSharedPtr msg); void onOccupancyGrid(const OccupancyGrid::ConstSharedPtr msg); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 781c59bcc3901..80be249ee1209 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -76,6 +76,9 @@ struct AvoidanceParameters // enable yield maneuver. bool enable_yield_maneuver{false}; + // disable path update + bool disable_path_update{false}; + // constrains bool use_constraints_for_decel{false}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 7c47caf2dc90d..342f2c8983cac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -105,7 +105,7 @@ class PullOutModule : public SceneModuleInterface std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; ParallelParkingParameters getGeometricPullOutParameters() const; - std::vector searchBackedPoses(); + std::vector searchPullOutStartPoses(); std::shared_ptr lane_departure_checker_; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 9847287793725..29b522cd3b919 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -324,6 +324,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() declare_parameter(ns + "enable_update_path_when_object_is_gone"); p.enable_safety_check = declare_parameter(ns + "enable_safety_check"); p.enable_yield_maneuver = declare_parameter(ns + "enable_yield_maneuver"); + p.disable_path_update = declare_parameter(ns + "disable_path_update"); p.publish_debug_marker = declare_parameter(ns + "publish_debug_marker"); p.print_debug_info = declare_parameter(ns + "print_debug_info"); } @@ -1017,7 +1018,7 @@ bool BehaviorPathPlannerNode::keepInputPoints( return false; } -void BehaviorPathPlannerNode::onOdometry(const Odometry::SharedPtr msg) +void BehaviorPathPlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) { const std::lock_guard lock(mutex_pd_); planner_data_->self_odometry = msg; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3c8ffe389aca6..a364fff2ca269 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -70,6 +70,21 @@ AvoidLine getNonStraightShiftLine(const AvoidLineArray & shift_lines) return {}; } +bool isEndPointsConnected( + const lanelet::ConstLanelet & left_lane, const lanelet::ConstLanelet & right_lane) +{ + const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint(); + const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint(); + + constexpr double epsilon = 1e-5; + return (right_back_point_2d - left_back_point_2d).norm() < epsilon; +} + +template +void pushUniqueVector(T & base_vector, const T & additional_vector) +{ + base_vector.insert(base_vector.end(), additional_vector.begin(), additional_vector.end()); +} } // namespace AvoidanceModule::AvoidanceModule( @@ -93,6 +108,18 @@ bool AvoidanceModule::isExecutionRequested() const return true; } + // Check ego is in preferred lane + const auto current_lanes = util::getCurrentLanes(planner_data_); + lanelet::ConstLanelet current_lane; + lanelet::utils::query::getClosestLanelet( + current_lanes, planner_data_->self_odometry->pose.pose, ¤t_lane); + const auto num = planner_data_->route_handler->getNumLaneToPreferredLane(current_lane); + + if (num != 0) { + return false; + } + + // Check avoidance targets exist const auto avoid_data = calcAvoidancePlanningData(debug_data_); if (parameters_->publish_debug_marker) { @@ -320,7 +347,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // object is behind ego or too far. if (object_data.longitudinal < -parameters_->object_check_backward_distance) { avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); - object_data.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + object_data.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; data.other_objects.push_back(object_data); continue; } @@ -2534,39 +2561,34 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const continue; } - // get left side lane - const lanelet::ConstLanelets all_left_lanelets = - route_handler->getAllLeftSharedLinestringLanelets(current_lane, enable_opposite, true); - if (!all_left_lanelets.empty()) { - current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet - - for (int i = all_left_lanelets.size() - 2; i >= 0; --i) { - current_drivable_lanes.middle_lanes.push_back(all_left_lanelets.at(i)); + // 1. get left/right side lanes + const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_left_lanelets = + route_handler->getAllLeftSharedLinestringLanelets(target_lane, enable_opposite, true); + if (!all_left_lanelets.empty()) { + current_drivable_lanes.left_lane = all_left_lanelets.back(); // leftmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_left_lanelets.begin(), all_left_lanelets.end() - 1)); } - } - - // get right side lane - const lanelet::ConstLanelets all_right_lanelets = - route_handler->getAllRightSharedLinestringLanelets(current_lane, enable_opposite, true); - if (!all_right_lanelets.empty()) { - current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet - if (current_drivable_lanes.left_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = + route_handler->getAllRightSharedLinestringLanelets(target_lane, enable_opposite, true); + if (!all_right_lanelets.empty()) { + current_drivable_lanes.right_lane = all_right_lanelets.back(); // rightmost lanelet + pushUniqueVector( + current_drivable_lanes.middle_lanes, + lanelet::ConstLanelets(all_right_lanelets.begin(), all_right_lanelets.end() - 1)); } + }; - for (size_t i = 0; i < all_right_lanelets.size() - 1; ++i) { - current_drivable_lanes.middle_lanes.push_back(all_right_lanelets.at(i)); - } - } + update_left_lanelets(current_lane); + update_right_lanelets(current_lane); - // 2. when there are multiple turning lanes whose previous lanelet is the same in - // intersection - const lanelet::ConstLanelets next_lanes_from_intersection = std::invoke( + // 2.1 when there are multiple lanes whose previous lanelet is the same + const auto get_next_lanes_from_same_previous_lane = [&route_handler](const lanelet::ConstLanelet & lane) { - if (!lane.hasAttribute("turn_direction")) { - return lanelet::ConstLanelets{}; - } - // get previous lane, and return false if previous lane does not exist lanelet::ConstLanelets prev_lanes; if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { @@ -2576,42 +2598,86 @@ void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const lanelet::ConstLanelets next_lanes; for (const auto & prev_lane : prev_lanes) { const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); - next_lanes.reserve(next_lanes.size() + next_lanes_from_prev.size()); - next_lanes.insert( - next_lanes.end(), next_lanes_from_prev.begin(), next_lanes_from_prev.end()); + pushUniqueVector(next_lanes, next_lanes_from_prev); } return next_lanes; - }, - current_lane); + }; - // 2.1 look for neighbour lane, where end line of the lane is connected to end line of the - // original lane - for (const auto & next_lane : next_lanes_from_intersection) { - if (current_lane.id() == next_lane.id()) { - continue; - } - constexpr double epsilon = 1e-5; - const auto & next_left_back_point_2d = next_lane.leftBound2d().back().basicPoint(); - const auto & next_right_back_point_2d = next_lane.rightBound2d().back().basicPoint(); - const auto & orig_left_back_point_2d = current_lane.leftBound2d().back().basicPoint(); - const auto & orig_right_back_point_2d = current_lane.rightBound2d().back().basicPoint(); - - if ((next_right_back_point_2d - orig_left_back_point_2d).norm() < epsilon) { - current_drivable_lanes.left_lane = next_lane; - if ( - current_drivable_lanes.right_lane.id() != current_lane.id() && - !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + const auto next_lanes_for_right = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.right_lane); + const auto next_lanes_for_left = + get_next_lanes_from_same_previous_lane(current_drivable_lanes.left_lane); + + // 2.2 look for neighbour lane recursively, where end line of the lane is connected to end line + // of the original lane + const auto update_drivable_lanes = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + for (const auto & next_lane : next_lanes) { + const auto & edge_lane = + is_left ? current_drivable_lanes.left_lane : current_drivable_lanes.right_lane; + if (next_lane.id() == edge_lane.id()) { + continue; + } + + const auto & left_lane = is_left ? next_lane : edge_lane; + const auto & right_lane = is_left ? edge_lane : next_lane; + if (!isEndPointsConnected(left_lane, right_lane)) { + continue; + } + + if (is_left) { + current_drivable_lanes.left_lane = next_lane; + } else { + current_drivable_lanes.right_lane = next_lane; + } + + if (!has_same_lane(current_drivable_lanes.middle_lanes, edge_lane)) { + if (is_left) { + if (current_drivable_lanes.right_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } else { + if (current_drivable_lanes.left_lane.id() != edge_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(edge_lane); + } + } + } + + return true; } - } else if ( - (next_left_back_point_2d - orig_right_back_point_2d).norm() < epsilon && - !has_same_lane(current_drivable_lanes.middle_lanes, current_lane)) { - current_drivable_lanes.right_lane = next_lane; - if (current_drivable_lanes.left_lane.id() != current_lane.id()) { - current_drivable_lanes.middle_lanes.push_back(current_lane); + return false; + }; + + const auto expand_drivable_area_recursively = + [&](const lanelet::ConstLanelets & next_lanes, const bool is_left) { + // NOTE: set max search num to avoid infinity loop for drivable area expansion + constexpr size_t max_recursive_search_num = 3; + for (size_t i = 0; i < max_recursive_search_num; ++i) { + const bool is_update_kept = update_drivable_lanes(next_lanes, is_left); + if (!is_update_kept) { + break; + } + if (i == max_recursive_search_num - 1) { + RCLCPP_ERROR( + rclcpp::get_logger("behavior_path_planner").get_child("avoidance"), + "Drivable area expansion reaches max iteration."); + } } - } + }; + expand_drivable_area_recursively(next_lanes_for_right, false); + expand_drivable_area_recursively(next_lanes_for_left, true); + + // 3. update again for new left/right lanes + update_left_lanelets(current_drivable_lanes.left_lane); + update_right_lanelets(current_drivable_lanes.right_lane); + + // 4. compensate that current_lane is in either of left_lane, right_lane or middle_lanes. + if ( + current_drivable_lanes.left_lane.id() != current_lane.id() && + current_drivable_lanes.right_lane.id() != current_lane.id()) { + current_drivable_lanes.middle_lanes.push_back(current_lane); } + drivable_lanes.push_back(current_drivable_lanes); } @@ -2851,7 +2917,9 @@ BehaviorModuleOutput AvoidanceModule::plan() } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(data.safe_new_sl); + if (!parameters_->disable_path_update) { + addShiftLineIfApproved(data.safe_new_sl); + } } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2882,7 +2950,9 @@ BehaviorModuleOutput AvoidanceModule::plan() } avoidance_data_.state = updateEgoState(data); - updateEgoBehavior(data, avoidance_path); + if (!parameters_->disable_path_update) { + updateEgoBehavior(data, avoidance_path); + } if (parameters_->publish_debug_marker) { setDebugData(avoidance_data_, path_shifter_, debug_data_); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp index 83263cb423a68..6af7a4aead4f3 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_utils.cpp @@ -232,6 +232,38 @@ double calcDecelDistPlanType3(const double v0, const double a0, const double ja) return x1; } +tier4_autoware_utils::Polygon2d expandPolygon( + const tier4_autoware_utils::Polygon2d & input_polygon, const double offset) +{ + // NOTE: There is a duplicated point. + const size_t num_points = input_polygon.outer().size() - 1; + + tier4_autoware_utils::Polygon2d expanded_polygon; + for (size_t i = 0; i < num_points; ++i) { + const auto & curr_p = input_polygon.outer().at(i); + const auto & next_p = input_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? input_polygon.outer().at(num_points - 1) : input_polygon.outer().at(i - 1); + + Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + current_to_next.normalize(); + current_to_prev.normalize(); + + const Eigen::Vector2d offset_vector = (-current_to_next - current_to_prev).normalized(); + const double theta = std::acos(offset_vector.dot(current_to_next)); + const double scaled_offset = offset / std::sin(theta); + const Eigen::Vector2d offset_point = + Eigen::Vector2d(curr_p.x(), curr_p.y()) + offset_vector * scaled_offset; + + expanded_polygon.outer().push_back( + tier4_autoware_utils::Point2d(offset_point.x(), offset_point.y())); + } + + boost::geometry::correct(expanded_polygon); + return expanded_polygon; +} + } // namespace bool isOnRight(const ObjectData & obj) { return obj.lateral < 0.0; } @@ -503,42 +535,8 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - std::vector offset_polygons{}; - bg::strategy::buffer::distance_symmetric distance_strategy(envelope_buffer); - bg::strategy::buffer::join_miter join_strategy; - bg::strategy::buffer::end_flat end_strategy; - bg::strategy::buffer::side_straight side_strategy; - bg::strategy::buffer::point_circle point_strategy; - bg::buffer( - toPolygon2d(envelope_ros_polygon), offset_polygons, distance_strategy, side_strategy, - join_strategy, end_strategy, point_strategy); - - return offset_polygons.front(); -} - -void getEdgePoints( - const Polygon2d & object_polygon, const double threshold, std::vector & edge_points) -{ - if (object_polygon.outer().size() < 2) { - return; - } - - const size_t num_points = object_polygon.outer().size(); - for (size_t i = 0; i < num_points - 1; ++i) { - const auto & curr_p = object_polygon.outer().at(i); - const auto & next_p = object_polygon.outer().at(i + 1); - const auto & prev_p = - i == 0 ? object_polygon.outer().at(num_points - 2) : object_polygon.outer().at(i - 1); - const Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); - const Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); - const double inner_val = current_to_next.dot(current_to_prev); - if (std::fabs(inner_val) > threshold) { - continue; - } - - const auto edge_point = tier4_autoware_utils::createPoint(curr_p.x(), curr_p.y(), 0.0); - edge_points.push_back(edge_point); - } + const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + return expanded_polygon; } void sortPolygonPoints( @@ -645,24 +643,28 @@ std::vector updateBoundary( const auto closest_end_point = motion_utils::calcLongitudinalOffsetPoint(original_bound, end_segment_idx, end_offset); - std::vector updated_bound; - const double min_dist = 1e-3; - // copy original points until front point - std::copy( - original_bound.begin(), original_bound.begin() + start_segment_idx + 1, - std::back_inserter(updated_bound)); - // insert closest front point - if ( - closest_front_point && - tier4_autoware_utils::calcDistance2d(*closest_front_point, updated_bound.back()) > min_dist) { - updated_bound.push_back(*closest_front_point); + std::vector updated_bound; + if (0 < front_offset) { + // copy original points until front point + std::copy( + original_bound.begin(), original_bound.begin() + start_segment_idx + 1, + std::back_inserter(updated_bound)); + + // insert closest front point + if ( + closest_front_point && + tier4_autoware_utils::calcDistance2d(*closest_front_point, updated_bound.back()) > min_dist) { + updated_bound.push_back(*closest_front_point); + } } // insert sorted points for (const auto & sorted_point : sorted_points) { - if (tier4_autoware_utils::calcDistance2d(sorted_point.point, updated_bound.back()) > min_dist) { + if ( + updated_bound.empty() || + tier4_autoware_utils::calcDistance2d(sorted_point.point, updated_bound.back()) > min_dist) { updated_bound.push_back(sorted_point.point); } } @@ -670,13 +672,15 @@ std::vector updateBoundary( // insert closest end point if ( closest_end_point && - tier4_autoware_utils::calcDistance2d(*closest_end_point, updated_bound.back()) > min_dist) { + (updated_bound.empty() || + tier4_autoware_utils::calcDistance2d(*closest_end_point, updated_bound.back()) > min_dist)) { updated_bound.push_back(*closest_end_point); } // copy original points until the end of the original bound for (size_t i = end_segment_idx + 1; i < original_bound.size(); ++i) { if ( + updated_bound.empty() || tier4_autoware_utils::calcDistance2d(original_bound.at(i), updated_bound.back()) > min_dist) { updated_bound.push_back(original_bound.at(i)); } @@ -698,11 +702,17 @@ void generateDrivableArea( for (const auto & object : objects) { const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; const auto & obj_poly = object.envelope_poly; - constexpr double threshold = 0.01; // get edge points of the object + const size_t nearest_path_idx = motion_utils::findNearestIndex( + path.points, obj_pose.position); // to get z for object polygon std::vector edge_points; - getEdgePoints(obj_poly, threshold, edge_points); + for (size_t i = 0; i < obj_poly.outer().size() - 1; + ++i) { // NOTE: There is a duplicated points + edge_points.push_back(tier4_autoware_utils::createPoint( + obj_poly.outer().at(i).x(), obj_poly.outer().at(i).y(), + path.points.at(nearest_path_idx).point.pose.position.z)); + } // get a boundary that we have to change const double lat_dist_to_path = motion_utils::calcLateralOffset(path.points, obj_pose.position); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 1059a1187684c..98d7d4d837c85 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -44,6 +44,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::util::calcObjectPolygon; using behavior_path_planner::util::getHighestProbLabel; using geometry_msgs::msg::Pose; +using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -112,6 +113,66 @@ void filterObjectIndices( } } } + +std::vector> getSortedLaneIds( + const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes, const double rough_shift_length) +{ + std::vector> sorted_lane_ids{}; + sorted_lane_ids.reserve(target_lanes.size()); + const auto get_sorted_lane_ids = [&](const lanelet::ConstLanelet & target_lane) { + const auto routing_graph_ptr = route_handler.getRoutingGraphPtr(); + lanelet::ConstLanelet lane; + if (rough_shift_length < 0.0) { + // lane change to the left, so I wan to take the lane right to target + const auto has_target_right = routing_graph_ptr->right(target_lane); + if (has_target_right) { + lane = *has_target_right; + } + } else if (rough_shift_length > 0.0) { + const auto has_target_left = routing_graph_ptr->left(target_lane); + if (has_target_left) { + lane = *has_target_left; + } + } else { + lane = target_lane; + } + + const auto find_same_id = std::find_if( + current_lanes.cbegin(), current_lanes.cend(), + [&lane](const lanelet::ConstLanelet & orig) { return orig.id() == lane.id(); }); + + if (find_same_id == current_lanes.cend()) { + return std::vector{target_lane.id()}; + } + + if (target_lane.id() > find_same_id->id()) { + return std::vector{find_same_id->id(), target_lane.id()}; + } + + return std::vector{target_lane.id(), find_same_id->id()}; + }; + + std::transform( + target_lanes.cbegin(), target_lanes.cend(), std::back_inserter(sorted_lane_ids), + get_sorted_lane_ids); + + return sorted_lane_ids; +} + +std::vector replaceWithSortedIds( + const std::vector & original_lane_ids, + const std::vector> & sorted_lane_ids) +{ + for (const auto original_id : original_lane_ids) { + for (const auto & sorted_id : sorted_lane_ids) { + if (std::find(sorted_id.cbegin(), sorted_id.cend(), original_id) != sorted_id.cend()) { + return sorted_id; + } + } + } + return original_lane_ids; +} } // namespace namespace behavior_path_planner::lane_change_utils @@ -181,7 +242,8 @@ std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, - const double acceleration, const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, + const std::vector> & sorted_lane_ids, const double acceleration, + const LaneChangePhaseInfo distance, const LaneChangePhaseInfo speed, const LaneChangeParameters & lane_change_param) { PathShifter path_shifter; @@ -236,41 +298,38 @@ std::optional constructCandidatePath( const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lanechange_end_idx = motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); - const auto insertLaneIDs = [](auto & target, const auto src) { - target.lane_ids.insert(target.lane_ids.end(), src.lane_ids.begin(), src.lane_ids.end()); - }; - if (lanechange_end_idx) { - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *lanechange_end_idx) { - insertLaneIDs(point, lane_changing_start_point); - insertLaneIDs(point, lane_changing_end_point); - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - lane_changing_start_point.point.longitudinal_velocity_mps); - continue; - } - point.point.longitudinal_velocity_mps = - std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); - const auto nearest_idx = - motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); - point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; - } - candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); - candidate_path.shifted_path = shifted_path; - } else { + if (!lanechange_end_idx) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "lane change end idx not found on target path."); return std::nullopt; } - // check candidate path is in lanelet - if (!isPathInLanelets(candidate_path.path, original_lanelets, target_lanelets)) { + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < *lanechange_end_idx) { + point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, + lane_changing_start_point.point.longitudinal_velocity_mps); + continue; + } + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(lane_changing_speed)); + const auto nearest_idx = + motion_utils::findNearestIndex(lane_changing_segment.points, point.point.pose); + point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; + } + + if (!isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { return std::nullopt; } + // check candidate path is in lanelet + candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); + candidate_path.shifted_path = shifted_path; + return std::optional{candidate_path}; } @@ -322,6 +381,9 @@ LaneChangePaths getLaneChangePaths( const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets); + const auto sorted_lane_ids = getSortedLaneIds( + route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); + for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { const double prepare_speed = getExpectedVelocityWhenDecelerate( @@ -385,7 +447,8 @@ LaneChangePaths getLaneChangePaths( const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, acceleration, lc_dist, lc_speed, parameter); + shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, + lc_speed, parameter); if (!candidate_path) { continue; @@ -478,7 +541,9 @@ bool hasEnoughDistance( return true; } - if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, target_lanes)) { + if ( + lane_change_total_distance + lane_change_required_distance > + util::getDistanceToEndOfLane(current_pose, target_lanes)) { return false; } diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 9e96a502561c2..2daa1ecf153d6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -389,13 +389,13 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } // plan with each planner for (const auto & planner : pull_out_planners_) { - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); planner->setPlannerData(planner_data_); @@ -412,6 +412,9 @@ void PullOutModule::planWithPriorityOnEfficientPath( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); @@ -439,11 +442,11 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = PlannerType::NONE; // check if start pose candidates are valid - if (start_pose_candidates.size() < 2) { + if (start_pose_candidates.empty()) { return; } - for (size_t i = 0; i < start_pose_candidates.size() - 1; i++) { + for (size_t i = 0; i < start_pose_candidates.size(); i++) { status_.back_finished = i == 0; const auto & pull_out_start_pose = start_pose_candidates.at(i); // plan with each planner @@ -462,6 +465,9 @@ void PullOutModule::planWithPriorityOnShortBackDistance( status_.planner_type = planner->getPlannerType(); break; } + + if (i == start_pose_candidates.size() - 1) continue; + // check next path if back is needed const auto & pull_out_start_pose_next = start_pose_candidates.at(i + 1); const auto pull_out_path_next = planner->plan(pull_out_start_pose_next, goal_pose); @@ -528,16 +534,7 @@ void PullOutModule::updatePullOutStatus() util::generateDrivableLanesWithShoulderLanes(status_.current_lanes, status_.pull_out_lanes); // search pull out start candidates backward - std::vector start_pose_candidates; - { - if (parameters_.enable_back) { - // the first element is current_pose - start_pose_candidates = searchBackedPoses(); - } else { - // pull_out_start candidate is only current pose - start_pose_candidates.push_back(current_pose); - } - } + std::vector start_pose_candidates = searchPullOutStartPoses(); if (parameters_.search_priority == "efficient_path") { planWithPriorityOnEfficientPath(start_pose_candidates, goal_pose); @@ -574,8 +571,10 @@ void PullOutModule::updatePullOutStatus() } // make this class? -std::vector PullOutModule::searchBackedPoses() +std::vector PullOutModule::searchPullOutStartPoses() { + std::vector pull_out_start_pose{}; + const Pose & current_pose = planner_data_->self_odometry->pose.pose; // get backward shoulder path @@ -592,9 +591,18 @@ std::vector PullOutModule::searchBackedPoses() p.point.pose = calcOffsetPose(p.point.pose, 0, distance_from_center_line, 0); } + // if backward driving is disable, just refine current pose to the lanes + if (!parameters_.enable_back) { + const auto refined_pose = + calcLongitudinalOffsetPose(backward_shoulder_path.points, current_pose.position, 0); + if (refined_pose) { + pull_out_start_pose.push_back(*refined_pose); + } + return pull_out_start_pose; + } + // check collision between footprint and object at the backed pose const auto local_vehicle_footprint = createVehicleFootprint(vehicle_info_); - std::vector backed_poses; for (double back_distance = 0.0; back_distance <= parameters_.max_back_distance; back_distance += parameters_.backward_search_resolution) { const auto backed_pose = calcLongitudinalOffsetPose( @@ -624,9 +632,9 @@ std::vector PullOutModule::searchBackedPoses() break; // poses behind this has a collision, so break. } - backed_poses.push_back(*backed_pose); + pull_out_start_pose.push_back(*backed_pose); } - return backed_poses; + return pull_out_start_pose; } bool PullOutModule::isOverlappedWithLane( diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index d5fb98aa8e980..58bf256f5f202 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -183,19 +183,25 @@ std::vector ShiftPullOut::calcPullOutPaths( } // get shift end pose - const auto shift_end_pose = std::invoke([&]() { + const auto shift_end_pose_ptr = std::invoke([&]() { const auto arc_position_shift_start = lanelet::utils::getArcCoordinates(road_lanes, start_pose); const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance; const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); - return path.points.front().point.pose; + return path.points.empty() + ? nullptr + : std::make_shared(path.points.front().point.pose); }); + if (!shift_end_pose_ptr) { + continue; + } + // create shift line ShiftLine shift_line{}; shift_line.start = start_pose; - shift_line.end = shift_end_pose; + shift_line.end = *shift_end_pose_ptr; shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); @@ -208,7 +214,7 @@ std::vector ShiftPullOut::calcPullOutPaths( // set velocity const size_t pull_out_end_idx = - findNearestIndex(shifted_path.path.points, shift_end_pose.position); + findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); if (i < pull_out_end_idx) { diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 15fa153221820..913139774136f 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -138,8 +138,8 @@ std::vector GeometricParallelParking::generatePullOverPaths( // check the continuity of straight path and arc path const Pose & road_path_last_pose = straight_path.points.back().point.pose; const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; - const double yaw_diff = tier4_autoware_utils::normalizeRadian( - tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation)); + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_last_pose.orientation) - tf2::getYaw(arc_path_first_pose.orientation))); const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { return std::vector{}; @@ -163,8 +163,6 @@ bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) { - clearPaths(); - const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance : parameters_.after_backward_parking_straight_distance; @@ -227,8 +225,6 @@ bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes) { - clearPaths(); - constexpr bool is_forward = false; // parking backward means departing forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose constexpr double max_offset = 10.0; @@ -267,7 +263,7 @@ bool GeometricParallelParking::planPullOut( } // get road center line path from departing end to goal, and combine after the second arc path - const double s_start = getArcCoordinates(road_lanes, *end_pose).length + 1.0; // need buffer? + const double s_start = getArcCoordinates(road_lanes, *end_pose).length; const double s_goal = getArcCoordinates(road_lanes, goal_pose).length; const double road_lanes_length = std::accumulate( road_lanes.begin(), road_lanes.end(), 0.0, [](const double sum, const auto & lane) { @@ -278,6 +274,16 @@ bool GeometricParallelParking::planPullOut( PathWithLaneId road_center_line_path = planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + // check the continuity of straight path and arc path + const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; + const Pose & arc_path_last_pose = arc_paths.back().points.back().point.pose; + const double yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_first_pose.orientation) - tf2::getYaw(arc_path_last_pose.orientation))); + const double distance = calcDistance2d(road_path_first_pose, arc_path_last_pose); + if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + continue; + } + // set departing velocity to arc paths and 0 velocity to end point constexpr bool set_stop_end = false; setVelocityToArcPaths(arc_paths, parameters_.departing_velocity, set_stop_end); @@ -286,7 +292,8 @@ bool GeometricParallelParking::planPullOut( // combine the road center line path with the second arc path auto paths = arc_paths; paths.back().points.insert( - paths.back().points.end(), road_center_line_path.points.begin(), + paths.back().points.end(), + road_center_line_path.points.begin() + 1, // to avoid overlapped point road_center_line_path.points.end()); removeOverlappingPoints(paths.back()); @@ -350,6 +357,8 @@ std::vector GeometricParallelParking::planOneTrial( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const double end_pose_offset, const double lane_departure_margin) { + clearPaths(); + const auto common_params = planner_data_->parameters; const Pose arc_end_pose = calcOffsetPose(goal_pose, end_pose_offset, 0, 0); @@ -373,7 +382,16 @@ std::vector GeometricParallelParking::planOneTrial( } // combine road and shoulder lanes - lanelet::ConstLanelets lanes = road_lanes; + // cut the road lanes up to start_pose to prevent unintended processing for overlapped lane + lanelet::ConstLanelets lanes{}; + tier4_autoware_utils::Point2d start_point2d(start_pose.position.x, start_pose.position.y); + for (const auto & lane : road_lanes) { + if (boost::geometry::within(start_point2d, lane.polygon2d().basicPolygon())) { + lanes.push_back(lane); + break; + } + lanes.push_back(lane); + } lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 156bca488cc58..f5f2987f63206 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1088,9 +1088,9 @@ void generateDrivableArea( }; const auto has_overlap = - [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { + [&](const lanelet::ConstLanelet & lane, const lanelet::ConstLanelets & ignore_lanelets = {}) { for (const auto & transformed_lane : transformed_lanes) { - if (transformed_lane.id() == ignore_lane_id) { + if (checkHasSameLane(ignore_lanelets, transformed_lane)) { continue; } if (boost::geometry::intersects( @@ -1114,6 +1114,16 @@ void generateDrivableArea( checkHasSameLane(transformed_lanes, goal_lanelet)) { const auto lanes_after_goal = route_handler->getLanesAfterGoal(vehicle_length); const auto next_lanes_after_goal = route_handler->getNextLanelets(goal_lanelet); + const auto goal_left_lanelet = route_handler->getLeftLanelet(goal_lanelet); + const auto goal_right_lanelet = route_handler->getRightLanelet(goal_lanelet); + lanelet::ConstLanelets goal_lanelets = {goal_lanelet}; + if (goal_left_lanelet) { + goal_lanelets.push_back(*goal_left_lanelet); + } + if (goal_right_lanelet) { + goal_lanelets.push_back(*goal_right_lanelet); + } + for (const auto & lane : lanes_after_goal) { // If lane is already in the transformed lanes, ignore it if (checkHasSameLane(transformed_lanes, lane)) { @@ -1121,9 +1131,8 @@ void generateDrivableArea( } // Check if overlapped const bool is_overlapped = - (checkHasSameLane(next_lanes_after_goal, lane) - ? has_overlap(lane, route_handler->getGoalLaneId()) - : has_overlap(lane)); + (checkHasSameLane(next_lanes_after_goal, lane) ? has_overlap(lane, goal_lanelets) + : has_overlap(lane)); if (is_overlapped) { continue; } @@ -2026,25 +2035,7 @@ lanelet::ConstLanelets extendLanes( lanelet::ConstLanelets getExtendedCurrentLanes( const std::shared_ptr & planner_data) { - const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_odometry->pose.pose; - const auto common_parameters = planner_data->parameters; - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - - lanelet::ConstLanelet current_lane; - if (!route_handler->getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - RCLCPP_ERROR( - rclcpp::get_logger("behavior_path_planner").get_child("utilities"), - "failed to find closest lanelet within route!!!"); - return {}; // TODO(Horibe) what should be returned? - } - - // For current_lanes with desired length - const auto current_lanes = route_handler->getLaneletSequence( - current_lane, current_pose, common_parameters.backward_path_length, - common_parameters.forward_path_length); - - return extendLanes(route_handler, current_lanes); + return extendLanes(planner_data->route_handler, getCurrentLanes(planner_data)); } lanelet::ConstLanelets calcLaneAroundPose( @@ -2347,19 +2338,43 @@ bool hasEnoughDistance( const auto is_obj_in_front = isObjectFront(front_vehicle_pose); debug.is_front = is_obj_in_front; - const auto front_vehicle_velocity = - (is_obj_in_front) ? object_current_twist.linear : ego_current_twist.linear; + const auto [front_vehicle_velocity, rear_vehicle_velocity] = std::invoke([&]() { + debug.object_twist.linear = object_current_twist.linear; + if (is_obj_in_front) { + return std::make_pair( + util::l2Norm(object_current_twist.linear), util::l2Norm(ego_current_twist.linear)); + } + return std::make_pair( + util::l2Norm(ego_current_twist.linear), util::l2Norm(object_current_twist.linear)); + }); + + const auto is_unsafe_dist_between_vehicle = std::invoke([&]() { + // ignore this for parked vehicle. + if (l2Norm(object_current_twist.linear) < 0.1) { + return false; + } - const auto rear_vehicle_velocity = - (is_obj_in_front) ? ego_current_twist.linear : object_current_twist.linear; - debug.object_twist.linear = (is_obj_in_front) ? front_vehicle_velocity : rear_vehicle_velocity; + // the value guarantee distance between vehicles are always more than dist + const auto max_vel = std::max(front_vehicle_velocity, rear_vehicle_velocity); + constexpr auto scale = 0.8; + const auto dist = scale * std::abs(max_vel) + param.longitudinal_distance_min_threshold; + + // return value rounded to the nearest two floating point + return std::abs(front_vehicle_pose.position.x) < dist; + }); + + if (is_unsafe_dist_between_vehicle) { + return false; + } const auto front_vehicle_stop_threshold = frontVehicleStopDistance( - util::l2Norm(front_vehicle_velocity), front_decel, std::fabs(front_vehicle_pose.position.x)); + front_vehicle_velocity, front_decel, std::abs(front_vehicle_pose.position.x)); + // longitudinal_distance_min_threshold here guarantee future stopping distance must be more than + // longitudinal_distance_min_threshold const auto rear_vehicle_stop_threshold = std::max( rearVehicleStopDistance( - util::l2Norm(rear_vehicle_velocity), rear_decel, param.rear_vehicle_reaction_time, + rear_vehicle_velocity, rear_decel, param.rear_vehicle_reaction_time, param.rear_vehicle_safety_time_margin), param.longitudinal_distance_min_threshold); diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 9ed153bb1c3d4..c46861d32a384 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -3,6 +3,7 @@ intersection: state_transit_margin_time: 1.0 stop_line_margin: 3.0 + keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg new file mode 100644 index 0000000000000..640483a4ef2ac --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.drawio.svg @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedest... + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ + safe margin + +
+
+
+ + safe marg... + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + +
+
+
+ + collision point + +
+
+
+ + collision... + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{0}` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{n}` + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg deleted file mode 100644 index 45106037c084e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/behavior_after_safe_margin.svg +++ /dev/null @@ -1,454 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - - - - -
-
-
- pedestrian -
-
-
- - pedestrian - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- - safe margin - -
-
-
- - safe margin - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - -
-
-
- - collision point - -
-
-
- - collision point - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_{0} - - - - - - -
-
-
- - - - - - -
-
-
- - t_{n} - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg index 8a785bd919726..7c30818b54e77 100644 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/collision_free.drawio.svg @@ -1,110 +1,547 @@ - + + + - - - - - - - - - - - - - - - - - -
-
- ego + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ ego +
+
-
- + + ego + - - - - - - -
-
- pedestrian + + + + + + + + + + + + + + +
+
+
+ + collision free + +
+
-
- + + collision free + - - - - - - - - -
-
- pedestrian + + + + + +
+
+
+ pedestrian +
+
-
- + + pedes... + - - - - - - - - - - - - - -
-
- collision judgement is done by polygon consider  pedestrian radius + + + + + +
+
+
+ collision +
+
-
- + + collis... + - - - -
-
- collision free + + + + + +
+
+
+ pedestrian +
+
-
- + + pedes... + + + + + + + Text is not SVG - cannot display + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg new file mode 100644 index 0000000000000..3609eec53ff15 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/da.drawio.svg @@ -0,0 +1,596 @@ + + + + + + + + + + + + + +
+
+
+ wall +
+
+
+ + wall + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedes... + + + + + + + + +
+
+
+ TTV +
+
+
+ + TTV + + + + + +
+
+
+ lateral max distance +
+
+
+ + lateral m... + + + + + + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + + + + + +
+
+
+ offset +
+
+
+ + offset + + + + + + + + + + + + + + + + +
+
+
+ detection area max length +
+
+
+ + detection area max length + + + + + + + + + + +
+
+
+ TTC +
+
+
+ + TTC + + + + + +
+
+
+ + TTC>TTV + +
+
+
+ + + TTC>TTV + + + + + + +
+
+
+
+ TTV: time to vehicle +
+ +
+ TTC: time to collision +
+ +
+
+
+ + TTV: time to vehicleTTC: time to col... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg deleted file mode 100644 index b2e6772a6c02e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/detection_area_poly.svg +++ /dev/null @@ -1,567 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_4 - - - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - - - -
-
-
- offset -
-
-
- - offset - - - - - - - - -
-
-
- area which obstacle is coming  before ego vehicle pass -
-
-
- - area which obstacle is coming  befo... - - - - - -
-
-
- front bamper -
-
-
- - front ba... - - - - - - - - - - -
-
-
- lateral max distance -
-
-
- - lateral m... - - - - - - - - -
-
-
- calculate  lateral distance from TTC top consider case if obstacle coming out from here -
-
-
- - calculate  lateral distance from TTC top consider c... - - - - - - - - -
-
-
- - - - - - -
-
-
- - t_1 - - - - - - - - - - - - - - - - - -
-
-
- interpolated polygon -
-
-
- - interpolated polygon - - - - - - - -
-
-
- - - - - - -
-
-
- - t_4 - - - - - - - -
-
-
- expand detail -
-
-
- - expand detail - - - - - - - - - - - -
-
-
- collision point of obstacle -
-
-
- - collision point of obst... - - - - - -
-
-
- place where obstacle  might be coming -
-
-
- - place where obstacle  m... - - - - - - - - - - -
-
-
- area which ego vehicle pass before hidden obstacle is coming -
-
-
- - area which ego vehicle pass before... - - - - - - - -
-
-
- area which ego vehicle pass before hidden obstacle is coming -
-
-
- - area which ego vehicle pass before... - - - - - - - - - -
-
-
- detection area max length -
-
-
- - detection area max length - - - - - - - - -
-
-
- max length is calculated from ego current velocity and current acceleration -
-
-
- - max length is calculated from ego current velocity and current acceleration - - - - - - -
-
-
- area which planned velocity is not effective -
-
-
- - area which planned velocity i... - - - - - - -
-
-
- area which planned velocity is not effective -
-
-
- - area which planned velocity i... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg new file mode 100644 index 0000000000000..82a5d7b6eb30a --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg @@ -0,0 +1,3830 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ + `t` + + + + + + +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ + `v` + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + `t_{1}=\frac{a_max-a... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{0... + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{0... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1}=\frac{j}{6}*t... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2}=\frac{a_{max}... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `a_{0... + + + + + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `a_{1}... + + + + + + + + +
+
+
+ s +
+
+
+ + s + + + + + + +
+
+
+ v +
+
+
+ + v + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `s_{safe} =v_{1}*t_{1} +... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{3... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + `v_{safe} = 0`... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + , + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+ +
+ +
+
+
+ + + `t_{3} < t_{1}`... + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `x_{1... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$delay... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `t_{2... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$delay... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `const\ jer... + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + $$const \ accel$$ + + + + + + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `v_{2... + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg deleted file mode 100644 index 0d9f4f63ec203..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg +++ /dev/null @@ -1,1235 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - -
-
-
- - t - - - - - - -
-
-
- - - - - - -
-
-
- - v - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{1} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{1} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{2} - - - - - - -
-
-
- - - - - - -
-
-
- - const\ jerk - - - - - - -
-
-
- -
- - - - -
- -
-
-
- - const \ accel - - - - - - -
-
-
- - - - - - - - -
-
-
- - v_{1} - - - - - - -
-
-
- - - - - - - - -
- -
- -
-
-
- - t_{1}=\frac{a_max-a_{0}}{j_max}... - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{0} - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - v_{0} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{1}=\frac{j}{6}*t^3+\frac{a_{0... - - - - - - -
-
-
- - - - - - - - -
-
-
- - t_{2} - - - - - - -
-
-
- - - - - - - - -
-
-
- - x_{2}=\frac{a_{max}}{2}*t^2+v_{1... - - - - - - -
-
-
- - - - - - - - -
-
-
- - a_{0} - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg new file mode 100644 index 0000000000000..64a5d99b28baf --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/object_info_partition.drawio.svg @@ -0,0 +1,669 @@ + + + + + + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ lack of pcd +
+
+
+ + lack of pcd + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ fence +
+
+
+ + fence + + + + + +
+
+
+ we can tell there is no pedestrian +
+ run out from here +
+ if there is fence tag +
+
+
+ + we can tell there is no pedestrian... + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + +
+
+
+ lack of pcd +
+
+
+ + lack of pcd + + + + + + + + + + +
+
+
+ we can tell here is occupied cells +
+ if we use object information +
+
+
+ + we can tell here is occupied cells... + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg new file mode 100644 index 0000000000000..8acecba2f558d --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.drawio.svg @@ -0,0 +1,1053 @@ + + + + + + + + + + + + + + + + +
+
+
+ obstacle +
+
+
+ + obstacle + + + + + + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + ped... + + + + + + + + +
+
+
+ fence +
+
+
+ + fence + + + + + + + + + + + + + + + +
+
+
+ will not run out +
+
+
+ + wil... + + + + + + + + +
+
+
+ colliision +
+
+
+ + collii... + + + + + + + + + + + + + + + + + +
+
+
+ occupancy grid +
+
+
+ + occupancy grid + + + + + + + + + + + + + +
+
+
+ + occupied +
+ 58~100 + +
+
+
+ + occupie... + + + + + + + + + + + +
+
+
+ + + unknown +
+ 43~58 + + +
+
+
+ + unknown... + + + + + + + + + + + +
+
+
+ free space +
+ 0~43 +
+
+
+ + free sp... + + + + + + + + +
+
+
+ free space +
+
+
+ + free space + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ v [m/s] +
+
+
+ + v [m/s] + + + + + +
+
+
+ this module plans + safe slow down velocity +
+ to stop before collision with strong brake +
+
+
+ + this module plans safe slow down velocity... + + + + + + +
+
+
+ s [m] +
+
+
+ + s [m] + + + + + + + + + + + + + + + +
+
+
+ ego velocity +
+
+
+ + ego velocity + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg deleted file mode 100644 index aedeb20bd703e..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot_partition.svg +++ /dev/null @@ -1,145 +0,0 @@ - - - - - - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - - - -
-
-
-
- partition lanelet -
-
- - fence -
-
- - gurad_rail -
-
- - wall -
-
-
-
- - partition lanelet... - - - - - - -
-
-
-
detection area polygon
-
-
-
- - detection area polygon - - - - - - - - - - - -
-
-
- occlusion spot inside partition is considered -
-
-
- - occlusion spot inside p... - - - - - - -
-
-
- occlusion spot outside partition is not considered -
-
-
- - occlusion spot outside... - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg deleted file mode 100644 index 196f5a05bad0d..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/occupancy_grid.svg +++ /dev/null @@ -1,263 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
pedestrian
-
-
- - ped... - - - - - - - - -
-
-
- has collision -
-
-
- - has collision - - - - - - -
-
-
- has collision -
-
-
- - has collision - - - - - -
-
-
-

- - Occupancy Grid - -

-

- - - Occlusion spot is where N by N size "no information" grid cell has that doesn't have any collision with "occupied" cells from occlusion spot to ego path. - - -

-
-
-
-
-
-
- - Occupancy Grid... - - - - - -
-
-
-
- Occupancy Grid Cell -
-
-
- free space : 0~45 -
-
- unknown   : 45~55 -
-
- occupied   : 95~100 -
-
-
-
- - Occupancy Grid Cell... - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg deleted file mode 100644 index f3b14e972a8ae..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/possible_collision_info.svg +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - -
-
-
-

ObstacleInfo

-

- - Information of obstacle to consider as possible collision - if - there is a hidden obstacle that darting out from occlusion spot. -

-
-
-
- - ObstacleInfo... - - - - - - -
-
-
-

- Intersection -

-
- - position that ego vehicle can have a collision with hidden obstacle where ego vehicle baselink is at collision point. -
-
-
-
- - Intersection... - - - - - - -
-
-
-

- Collision Point -

-
- - position to inset safe velocity to path. -
-
-
- - Collision Point... - - - - - - - - - - - - - - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg index c6e1943c7bfbf..6dedeaac81142 100644 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/raycast_shadow.drawio.svg @@ -3,680 +3,749 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="647px" - height="682px" - viewBox="-0.5 -0.5 647 682" - content="<mxfile><diagram id="Doxya9YoADHyABFa7ko3" name="shadow">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</diagram></mxfile>" + width="655px" + height="824px" + viewBox="-0.5 -0.5 655 824" + content="<mxfile><diagram id="Doxya9YoADHyABFa7ko3" name="shadow">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</diagram></mxfile>" > - - - - - - - - - + + + + + + - - - - + + + + + + + + + + +
-
+
ego
- ego + ego - + + +
-
+
stuck vehicle
- stuck ve... + stuck... - + +
cruising -
+
vehicle
- cruising... + cruis... - - - - - - + + + +
-
possible collision
+
+ possible collision +
- possi... + pos... - - + + + + +
-
-
- not possible collision +
+
+ not possible collision
- not p... + not... - - - + + + + + +
-
- occupancy grid consider possible collision object ray cast +
+ remove possible collision candidate behind predicted objects
- occupancy grid consider possible collision object ray c... + remove possible collision candida... - - - - - - + + + + + + +
-
- +
+ passable -
- collision -
+
- passable... + passable - - - - - - + + + + + + + + + + + + + + + +
-
+
stuck vehicle
- stuck ve... + stuck... - + +
-
- passable +
+ passable
- passable + passable - + + +
-
+
cruising -
+
vehicle
- cruising... + cruis... - - + + + +
-
passable collision
+
+ passable collision +
- passa... + pas... - - + + + +
-
passable collision
+
+ passable collision +
- passa... + pas... + + + + + + + + + +
+
+
+ this pedestrian will collide with cruising car +
+
+
+ + this pedestrian will collide with... + + + + + + + - Viewer does not support full SVG 1.1 + Text is not SVG - cannot display diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg new file mode 100644 index 0000000000000..94951bec84f2c --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.drawio.svg @@ -0,0 +1,897 @@ + + + + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `S_{safe}("TTC")` + + + + + + + + + + +
+
+
+ pedestrian +
+
+
+ + pedes... + + + + + + + + +
+
+
+ Obstacle +
+
+
+ + Obstacle + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ s +
+
+
+ + s + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + `V_{safe}("TTC")` + + + + + + +
+
+
+ TTV +
+
+
+ + TTV + + + + + + +
+
+
+ +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC. +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + is the distance to stop when the car is running at + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + . +
+ + + TTV means Time To Vehicle for the approaching pedestrian to own vehicle. +
+ + TTC means Time To Collision of the approaching ego to collision point.. + +
+ +
+
+
+ + + `V_{safe}` is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC.... + + + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + +
+
+
+ collision point with margin +
+
+
+ + collision point with ma... + + + + + + +
+
+
+ collision point +
+
+
+ + collision point + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg deleted file mode 100644 index 7ee5bb292b90a..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/safe_motion.svg +++ /dev/null @@ -1,646 +0,0 @@ - - - - - - - - - - - - - - - - - - - -
-
-
- - - - - - - - -
-
-
- - S_{safe}("TTC") - - - - - - - -
-
-
pedestrian
-
-
- - pedes... - - - - - - -
-
-
- Obstacle -
-
-
- - Obstacle - - - - - - - - - - - - - - - - - - -
-
-
- s -
-
-
- - s - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- - - - - - - - -
-
-
- - V_{safe}("TTC") - - - - - - -
-
-
- TTC -
-
-
- - TTC - - - - - - -
-
-
- -
- - - - - - - - - is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC. -
- - - - - - - - - is the distance to stop when the car is running at - - - - - - - . -
- - TTC: TTC means Time To Collision of the approaching pedestrian with the own vehicle. -
-
-
- - - V_{safe} is calculated by emergency stop jerk and acceleration which ego vehicle can stop within TTC.... - - - - - - - -
-
-
- ego -
-
-
- - ego - - - - - - -
-
-
- collision point with margin -
-
-
- - collision point with ma... - - - - - - -
-
-
- collision point -
-
-
- - collision point - - - - - - - - - Text is not SVG - cannot display - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg deleted file mode 100644 index 5540ab7236750..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/sidewalk_slice.svg +++ /dev/null @@ -1,179 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
-

Sidewalk Slice

-
create sidewalk slice polygon inside focus range
-
-
-
- - Sidewalk Slice... - - - - - - - - -
-
-
- focus range -
-
-
- - focus range - - - - - -
-
-
-

Only Consider Lower Bound

-

considering all occlusion spot  cost very much, so consider only  longitudinally or laterally closest hidden obstacle as slow down  target.

-

-
-

-
-
-
- - Only Consider Lower Bound... - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg deleted file mode 100644 index a8afe81d7f497..0000000000000 --- a/planning/behavior_velocity_planner/docs/occlusion_spot/use_object_info.drawio.svg +++ /dev/null @@ -1,143 +0,0 @@ - - - - - - - - - - - - - -
-
- ego -
-
- - - - - - - -
-
- use object info -
-
- - - - - - - - - - - - -
-
- ego -
-
- - - - - - - -
-
- without object info -
-
- - - - - -
-
- obj -
-
- - - - - - - - - - - -
-
- obj -
-
- - - - - - - - - - - - - - diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg new file mode 100644 index 0000000000000..55693e2a2688c --- /dev/null +++ b/planning/behavior_velocity_planner/docs/occlusion_spot/velocity_planning.drawio.svg @@ -0,0 +1,1276 @@ + + + + + + + + + + + + + +
+
+
+ v [m/s] +
+
+
+ + v [m/s] + + + + + + + + + + + +
+
+
+ s [m] +
+
+
+ + s [m] + + + + + + + + + + + + + + + +
+
+
+ + min velocity + +
+
+
+ + min velocity + + + + + +
+
+
+ + + + - -  reference maximum velocity +
+
+ + ---  safe velocity to stop before collisopn + +
+ + + + + ---- safe slow donw velocity +
+ + + ---- min slow down velocity +
+ + + ---- motion velocity smoother velocity +
+ + O  current velocity +
+
+ + +
+
+
+ + - -  reference maximum velocity... + + + + + + + +
+
+
+ x +
+
+
+ + x + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ al +
+
+
+ + al + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp index c42a311ffa623..01c21d2197202 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/manager.hpp @@ -25,6 +25,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -43,7 +44,12 @@ class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; class MergeFromPrivateModuleManager : public SceneModuleManagerInterface @@ -61,7 +67,12 @@ class MergeFromPrivateModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; - bool hasSameParentLanelet(const lanelet::ConstLanelet & lane) const; + bool hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const; + + bool hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 90bdbea51297a..bd3f3c10cf0fa 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -67,6 +68,7 @@ class IntersectionModule : public SceneModuleInterface { double state_transit_margin_time; double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check double stuck_vehicle_ignore_dist; //! distance from intersection start to start stuck vehicle check @@ -114,6 +116,7 @@ class IntersectionModule : public SceneModuleInterface bool is_go_out_; // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; /** * @brief check collision for all lanelet area & predicted objects (call checkPathCollision() as diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp index a2d884240747d..6db961152d80e 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_merge_from_private_road.hpp @@ -95,6 +95,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Parameter PlannerParam planner_param_; + std::optional intersection_lanelets_; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp index 085de0c834ab7..6fa4d184239da 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util.hpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -52,17 +53,10 @@ std::optional getDuplicatedPointIdx( /** * @brief get objective polygons for detection area */ -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on = false); -struct StopLineIdx -{ - size_t first_inside_lane = 0; - size_t pass_judge_line = 0; - size_t stop_line = 0; -}; - /** * @brief Generate a stop line and insert it into the path. If the stop line is defined in the map, * read it from the map; otherwise, generate a stop line at a position where it will not collide. @@ -73,13 +67,27 @@ struct StopLineIdx " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane * @return nullopt if path is not intersecting with detection areas */ -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interaval, const rclcpp::Logger logger); + +/** + * @brief Generate a stop line for stuck vehicle + * @param conflicting_areas used to generate stop line for stuck vehicle + * @param original_path ego-car lane + * @param target_path target lane to insert stop point (part of ego-car lane or same to ego-car + * lane) + " @param use_stuck_stopline if true, a stop line is generated at the beginning of intersection lane + */ +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock); + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double ip_interval, + const std::pair lane_interval, const rclcpp::Logger logger); /** * @brief Calculate first path index that is in the polygon. @@ -136,7 +144,7 @@ bool isBeforeTargetIndex( const geometry_msgs::msg::Pose & current_pose, const int target_idx); lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane); std::optional getIntersectionArea( diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp new file mode 100644 index 0000000000000..604cefd373496 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/util_type.hpp @@ -0,0 +1,43 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ +#define SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ + +#include + +#include + +namespace behavior_velocity_planner::util +{ +struct IntersectionLanelets +{ + bool tl_arrow_solid_on; + lanelet::ConstLanelets attention; + lanelet::ConstLanelets conflicting; + lanelet::ConstLanelets adjacent; + std::vector attention_area; + std::vector conflicting_area; + std::vector adjacent_area; +}; + +struct StopLineIdx +{ + size_t pass_judge_line = 0; + size_t collision_stop_line = 0; +}; + +} // namespace behavior_velocity_planner::util + +#endif // SCENE_MODULE__INTERSECTION__UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp index a27d8b8d9e3c7..b93205e5acdb8 100644 --- a/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/traffic_light/manager.hpp @@ -43,7 +43,9 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; - bool isModuleRegisteredFromRegElement(const lanelet::Id & id) const; + bool isModuleRegisteredFromRegElement(const lanelet::Id & id, const size_t module_id) const; + + bool isModuleRegisteredFromExistingAssociatedModule(const lanelet::Id & id) const; bool hasSameTrafficLight( const lanelet::TrafficLightConstPtr element, diff --git a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp index a7fdc337d3dae..ce04fe9d5c7a9 100644 --- a/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner/include/utilization/trajectory_utils.hpp @@ -105,7 +105,7 @@ inline bool smoothPath( // Resample trajectory with ego-velocity based interval distances auto traj_resampled = smoother->resampleTrajectory( - *traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, + traj_lateral_acc_filtered, v0, current_pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints( traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold, diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md index b4670eef0345a..b05714cac1352 100644 --- a/planning/behavior_velocity_planner/occlusion-spot-design.md +++ b/planning/behavior_velocity_planner/occlusion-spot-design.md @@ -4,104 +4,108 @@ This module plans safe velocity to slow down before reaching collision point that hidden object is darting out from `occlusion spot` where driver can't see clearly because of obstacles. -![brief](./docs/occlusion_spot/occlusion_spot.svg) +![brief](./docs/occlusion_spot/occlusion_spot.drawio.svg) ### Activation Timing -This module is activated if `launch_occlusion_spot` becomes true +This module is activated if `launch_occlusion_spot` becomes true. To make pedestrian first zone map tag is one of the TODOs. -### Limitation +### Limitation and TODOs -To solve the excessive deceleration due to false positive of the perception, the logic of detection method can be selectable. This point has not been discussed in detail and needs to be improved (see the description below). +This module is prototype implementation to care occlusion spot. To solve the excessive deceleration due to false positive of the perception, the logic of detection method can be selectable. This point has not been discussed in detail and needs to be improved. + +- Make occupancy grid for planning. +- Make map tag for occlusion spot. +- About the best safe motion. + +TODOs are written in each Inner-workings / Algorithms (see the description below). ### Inner-workings / Algorithms #### Logics Working -There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **road with obstacles**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. This module considers all occlusion spots calculated from the **occupancy grid**, but it is not reasonable to take into account all occlusion spots for example, people jumping out from behind a guardrail, or behind cruising vehicle. Therefore currently detection area will be limited to to use **dynamic object** information. +There are several types of occlusions, such as "occlusions generated by parked vehicles" and "occlusions caused by obstructions". In situations such as driving on **road with obstacles**, where people jump out of the way frequently, all possible occlusion spots must be taken into account. This module considers all occlusion spots calculated from the **occupancy grid**, but it is not reasonable to take into account all occlusion spots for example, people jumping out from behind a guardrail, or behind cruising vehicle. Therefore currently detection area will be limited to to use **predicted object** information. Note that this decision logic is still under development and needs to be improved. -#### Occlusion Spot Public +#### DetectionArea Polygon -This module inserts safe velocity at the collision point estimated from the associated occlusion spot under assumption that the pedestrian possibly coming out of the occlusion spot. -![brief](./docs/occlusion_spot/possible_collision_info.svg) +This module considers TTV from pedestrian velocity and lateral distance to occlusion spot. +TTC is calculated from ego velocity and acceleration and longitudinal distance until collision point using motion velocity smoother. +To compute fast this module only consider occlusion spot whose TTV is less than TTC and only consider area within "max lateral distance". -This module consider 3 policies that are very important for risk predicting system for occlusion spot. +![brief](./docs/occlusion_spot/da.drawio.svg) -1. "Passable" without deceleration - If ego vehicle speed is high enough to pass the occlusion spot and expected to have no collision with any objects coming out of the occlusion spot, then it's possible for ego vehicle to pass the spot without deceleration. +#### Occlusion Spot Occupancy Grid Base -2. "Predictable" with enough distance to occlusion - If ego vehicle has enough distance to the occlusion spot, then ego vehicle is going to slow down to the speed that is slow enough to stop before collision with full brake. - If ego vehicle pass the possible collision point, then ego vehicle is going to drive normally. +This module considers any occlusion spot around ego path computed from the occupancy grid. +Due to the computational cost occupancy grid is not high resolution and this will make occupancy grid noisy so this module add information of occupancy to occupancy grid map. -3. "Unavoidable" without enough distance to occlusion spot - This module assumes the occlusion spot is detected stably far from the ego vehicle. Therefore this module can not guarantee the safety behavior for the occlusion spot detected suddenly in front of the ego vehicle. In this case, slow velocity that does not cause the strong deceleration is only applied. +TODO: consider hight of obstacle point cloud to generate occupancy grid. -#### Occlusion Spot Private +##### Collision Free Judgement -This module considers any occlusion spot around ego path computed from the occupancy grid. +obstacle that can run out from occlusion should have free space until intersection from ego vehicle -![occupancy_grid](./docs/occlusion_spot/occupancy_grid.svg) +![brief](./docs/occlusion_spot/collision_free.drawio.svg) -#### Occlusion Spot Common +##### Partition Lanelet -##### The Concept of Safe Velocity +By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. -The safe slowdown velocity is calculated from the below parameters of ego emergency braking system and time to collision. +By using static object information, it is possible to make occupancy grid more accurate. -- jerk limit[m/s^3] -- deceleration limit[m/s2] -- delay response time[s] -- time to collision of pedestrian[s] - with these parameters we can briefly define safe motion before occlusion spot for ideal environment. - ![occupancy_grid](./docs/occlusion_spot/safe_motion.svg) +To make occupancy grid for planning is one of the TODOs. -##### Maximum Slowdown Velocity +![brief](./docs/occlusion_spot/object_info_partition.drawio.svg) -The maximum slowdown velocity is calculated from the below parameters of ego current velocity and acceleration with maximum slowdown jerk and maximum slowdown acceleration in order not to slowdown too much. +##### Possible Collision -- $j_{max}$ slowdown jerk limit[m/s^3] -- $a_{max}$ slowdown deceleration limit[m/s2] -- $v_{0}$ current velocity[m/s] -- $a_{0}$ current acceleration[m/s] +obstacle that can run out from occlusion is interrupted by moving vehicle. -![brief](./docs/occlusion_spot/maximum_slowdown_velocity.svg) +![brief](./docs/occlusion_spot/raycast_shadow.drawio.svg) -##### Safe Behavior After Passing Safe Margin Point +#### About safe motion -This module defines safe margin to consider ego distance to stop and collision path point geometrically. -While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity. +![brief](./docs/occlusion_spot/velocity_planning.drawio.svg) -![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg) +##### The Concept of Safe Velocity and Margin -##### DetectionArea Polygon +The safe slowdown velocity is calculated from the below parameters of ego emergency braking system and time to collision. +Below calculation is included but change velocity dynamically is not recommended for planner. -Occlusion spot computation: searching occlusion spots for all cells in the occupancy_grid inside "max lateral distance" requires a lot of computational cost, so this module use only one most notable occlusion spot for each partition. (currently offset is from baselink to front for safety) -The maximum length of detection area depends on ego current vehicle velocity and acceleration. +- jerk limit[m/s^3] +- deceleration limit[m/s2] +- delay response time[s] +- time to collision of pedestrian[s] + with these parameters we can briefly define safe motion before occlusion spot for ideal environment. -![brief](./docs/occlusion_spot/detection_area_poly.svg) + ![occupancy_grid](./docs/occlusion_spot/safe_motion.drawio.svg) -##### Partition Lanelet +This module defines safe margin to consider ego distance to stop and collision path point geometrically. +While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity. -By using lanelet information of "guard_rail", "fence", "wall" tag, it's possible to remove unwanted occlusion spot. -![brief](./docs/occlusion_spot/occlusion_spot_partition.svg) +![brief](./docs/occlusion_spot/behavior_after_safe_margin.drawio.svg) -##### Use Object Info +Note: This logic assumes high-precision vehicle speed tracking and margin for decel point might not be the best solution, and override with manual driver is considered if pedestrian really run out from occlusion spot. -use object info to make occupancy grid more accurate -![brief](./docs/occlusion_spot/use_object_info.drawio.svg) +TODO: consider one of the best choices -##### Collision Free Judgement +1. stop in front of occlusion spot +2. insert 1km/h velocity in front of occlusion spot +3. slowdown this way +4. etc... . -obstacle that can run out from occlusion should have free space until intersection from ego vehicle -![brief](./docs/occlusion_spot/collision_free.drawio.svg) +##### Maximum Slowdown Velocity -##### Possible Collision +The maximum slowdown velocity is calculated from the below parameters of ego current velocity and acceleration with maximum slowdown jerk and maximum slowdown acceleration in order not to slowdown too much. -obstacle that can run out from occlusion is interrupted by moving vehicle. -![brief](./docs/occlusion_spot/raycast_shadow.drawio.svg) +- $j_{max}$ slowdown jerk limit[m/s^3] +- $a_{max}$ slowdown deceleration limit[m/s2] +- $v_{0}$ current velocity[m/s] +- $a_{0}$ current acceleration[m/s] + +![brief](./docs/occlusion_spot/maximum_slowdown_velocity.drawio.svg) #### Module Parameters @@ -222,7 +226,7 @@ stop @enduml ``` -##### Detail process for predicted object +##### Detail process for predicted object(not updated) ```plantuml @startuml @@ -274,7 +278,7 @@ stop @enduml ``` -##### Detail process for private road +##### Detail process for Occupancy grid base ```plantuml @startuml diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 9bf21a9d8fbea..126b629362161 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -324,9 +324,11 @@ void BehaviorVelocityPlannerNode::onAcceleration( void BehaviorVelocityPlannerNode::onParam() { + // Note(vrichard): mutex lock is not necessary as onParam is only called once in the constructed. + // It would be required if it was a callback. + // std::lock_guard lock(mutex_); planner_data_.velocity_smoother_ = std::make_unique(*this); - return; } void BehaviorVelocityPlannerNode::onLaneletMap( @@ -367,6 +369,7 @@ void BehaviorVelocityPlannerNode::onExternalIntersectionStates( void BehaviorVelocityPlannerNode::onExternalVelocityLimit(const VelocityLimit::ConstSharedPtr msg) { + std::lock_guard lock(mutex_); planner_data_.external_velocity_limit = *msg; } @@ -392,10 +395,9 @@ void BehaviorVelocityPlannerNode::onVirtualTrafficLightStates( void BehaviorVelocityPlannerNode::onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg) { - mutex_.lock(); // for planner_data_ + std::unique_lock lk(mutex_); if (!isDataReady(planner_data_, *get_clock())) { - mutex_.unlock(); return; } @@ -407,20 +409,17 @@ void BehaviorVelocityPlannerNode::onTrigger( if (!planner_data_.route_handler_) { RCLCPP_INFO_THROTTLE( get_logger(), *get_clock(), 3000, "Waiting for the initialization of route_handler"); - mutex_.unlock(); return; } - // NOTE: planner_data must not be referenced for multithreading - const auto planner_data = planner_data_; - mutex_.unlock(); - if (input_path_msg->points.empty()) { return; } const autoware_auto_planning_msgs::msg::Path output_path_msg = - generatePath(input_path_msg, planner_data); + generatePath(input_path_msg, planner_data_); + + lk.unlock(); path_pub_->publish(output_path_msg); stop_reason_diag_pub_->publish(planner_manager_.getStopReasonDiag()); diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index faa5bbcc06ea1..3de7279aeb27c 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -203,6 +203,73 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } +// calc smallest enclosing circle with average O(N) algorithm +// reference: +// https://erickimphotography.com/blog/wp-content/uploads/2018/09/Computational-Geometry-Algorithms-and-Applications-3rd-Ed.pdf +std::pair calcSmallestEnclosingCircle( + const lanelet::ConstPolygon2d & poly) +{ + // The `eps` is used to avoid precision bugs in circle inclusion checkings. + // If the value of `eps` is too small, this function doesn't work well. More than 1e-10 is + // recommended. + const double eps = 1e-5; + lanelet::BasicPoint2d center(0.0, 0.0); + double radius_squared = 0.0; + + auto cross = [](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> double { + return p1.x() * p2.y() - p1.y() * p2.x(); + }; + + auto make_circle_3 = [&]( + const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2, + const lanelet::BasicPoint2d & p3) -> void { + // reference for circumcenter vector https://en.wikipedia.org/wiki/Circumscribed_circle + const double A = (p2 - p3).squaredNorm(); + const double B = (p3 - p1).squaredNorm(); + const double C = (p1 - p2).squaredNorm(); + const double S = cross(p2 - p1, p3 - p1); + if (std::abs(S) < eps) return; + center = (A * (B + C - A) * p1 + B * (C + A - B) * p2 + C * (A + B - C) * p3) / (4 * S * S); + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto make_circle_2 = + [&](const lanelet::BasicPoint2d & p1, const lanelet::BasicPoint2d & p2) -> void { + center = (p1 + p2) * 0.5; + radius_squared = (center - p1).squaredNorm() + eps; + }; + + auto in_circle = [&](const lanelet::BasicPoint2d & p) -> bool { + return (center - p).squaredNorm() <= radius_squared; + }; + + // mini disc + for (size_t i = 1; i < poly.size(); i++) { + const auto p1 = poly[i].basicPoint2d(); + if (in_circle(p1)) continue; + + // mini disc with point + const auto p0 = poly[0].basicPoint2d(); + make_circle_2(p0, p1); + for (size_t j = 0; j < i; j++) { + const auto p2 = poly[j].basicPoint2d(); + if (in_circle(p2)) continue; + + // mini disc with two points + make_circle_2(p1, p2); + for (size_t k = 0; k < j; k++) { + const auto p3 = poly[k].basicPoint2d(); + if (in_circle(p3)) continue; + + // mini disc with tree points + make_circle_3(p1, p2, p3); + } + } + } + + return std::make_pair(center, radius_squared); +} + std::vector DetectionAreaModule::getObstaclePoints() const { std::vector obstacle_points; @@ -211,11 +278,17 @@ std::vector DetectionAreaModule::getObstaclePoints() const auto & points = *(planner_data_->no_ground_pointcloud); for (const auto & detection_area : detection_areas) { + const auto poly = lanelet::utils::to2D(detection_area); + const auto circle = calcSmallestEnclosingCircle(poly); for (const auto p : points) { - if (bg::within(Point2d{p.x, p.y}, lanelet::utils::to2D(detection_area).basicPolygon())) { - obstacle_points.push_back(planning_utils::toRosPoint(p)); - // get all obstacle point becomes high computation cost so skip if any point is found - break; + const double squared_dist = (circle.first.x() - p.x) * (circle.first.x() - p.x) + + (circle.first.y() - p.y) * (circle.first.y() - p.y); + if (squared_dist <= circle.second) { + if (bg::within(Point2d{p.x, p.y}, poly.basicPolygon())) { + obstacle_points.push_back(planning_utils::toRosPoint(p)); + // get all obstacle point becomes high computation cost so skip if any point is found + break; + } } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp index 8f4ee1c91cee2..f1289afd25b06 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/manager.cpp @@ -46,6 +46,7 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); ip.state_transit_margin_time = node.declare_parameter(ns + ".state_transit_margin_time", 2.0); ip.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); + ip.keep_detection_vel_thr = node.declare_parameter(ns + ".keep_detection_vel_thr", 0.833); ip.stuck_vehicle_detect_dist = node.declare_parameter(ns + ".stuck_vehicle_detect_dist", 3.0); ip.stuck_vehicle_ignore_dist = node.declare_parameter(ns + ".stuck_vehicle_ignore_dist", 5.0) + vehicle_info.max_longitudinal_offset_m; @@ -97,10 +98,6 @@ void IntersectionModuleManager::launchNewModules( const auto lane_id = ll.id(); const auto module_id = lane_id; - if (hasSameParentLanelet(ll)) { - continue; - } - // Is intersection? const std::string turn_direction = ll.attributeOr("turn_direction", "else"); const auto is_intersection = @@ -108,6 +105,11 @@ void IntersectionModuleManager::launchNewModules( if (!is_intersection) { continue; } + + if (hasSameParentLaneletAndTurnDirectionWithRegistered(ll, turn_direction)) { + continue; + } + registerModule(std::make_shared( module_id, lane_id, planner_data_, intersection_param_, logger_.get_child("intersection_module"), clock_)); @@ -142,29 +144,31 @@ void MergeFromPrivateModuleManager::launchNewModules( // Is merging from private road? // In case the goal is in private road, check if this lanelet is conflicting with urban lanelet const std::string lane_location = ll.attributeOr("location", "else"); - if (lane_location == "private") { - if (i + 1 < lanelets.size()) { - const auto next_lane = lanelets.at(i + 1); - const std::string next_lane_location = next_lane.attributeOr("location", "else"); - if (next_lane_location != "private") { + if (lane_location != "private") { + continue; + } + + if (i + 1 < lanelets.size()) { + const auto next_lane = lanelets.at(i + 1); + const std::string next_lane_location = next_lane.attributeOr("location", "else"); + if (next_lane_location != "private") { + registerModule(std::make_shared( + module_id, lane_id, planner_data_, merge_from_private_area_param_, + logger_.get_child("merge_from_private_road_module"), clock_)); + continue; + } + } else { + const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); + const auto conflicting_lanelets = + lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); + for (auto && conflicting_lanelet : conflicting_lanelets) { + const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); + if (conflicting_attr == "urban") { registerModule(std::make_shared( module_id, lane_id, planner_data_, merge_from_private_area_param_, logger_.get_child("merge_from_private_road_module"), clock_)); continue; } - } else { - const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); - const auto conflicting_lanelets = - lanelet::utils::getConflictingLanelets(routing_graph_ptr, ll); - for (auto && conflicting_lanelet : conflicting_lanelets) { - const std::string conflicting_attr = conflicting_lanelet.attributeOr("location", "else"); - if (conflicting_attr == "urban") { - registerModule(std::make_shared( - module_id, lane_id, planner_data_, merge_from_private_area_param_, - logger_.get_child("merge_from_private_road_module"), clock_)); - continue; - } - } } } } @@ -177,21 +181,21 @@ IntersectionModuleManager::getModuleExpiredFunction( const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return - [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - if (hasSameParentLanelet(lane)) { - return false; - } + return [this, lane_set](const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; } - return true; - }; + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { + return false; + } + } + return true; + }; } std::function &)> @@ -201,29 +205,56 @@ MergeFromPrivateModuleManager::getModuleExpiredFunction( const auto lane_set = planning_utils::getLaneletsOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return - [this, lane_set]([[maybe_unused]] const std::shared_ptr & scene_module) { - for (const auto & lane : lane_set) { - const std::string turn_direction = lane.attributeOr("turn_direction", "else"); - const auto is_intersection = - turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; - if (!is_intersection) { - continue; - } - if (hasSameParentLanelet(lane)) { - return false; - } + return [this, lane_set](const std::shared_ptr & scene_module) { + for (const auto & lane : lane_set) { + const std::string turn_direction = lane.attributeOr("turn_direction", "else"); + const auto is_intersection = + turn_direction == "right" || turn_direction == "left" || turn_direction == "straight"; + if (!is_intersection) { + continue; } + if (hasSameParentLaneletAndTurnDirectionWith( + lane, scene_module->getModuleId(), turn_direction)) { + return false; + } + } + return true; + }; +} + +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction_lane) const +{ + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction_lane) { + return false; + } + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); + lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { return true; - }; + } + } + return false; } -bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +bool IntersectionModuleManager::hasSameParentLaneletAndTurnDirectionWithRegistered( + const lanelet::ConstLanelet & lane, const std::string & turn_direction) const { lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); for (const auto & id : registered_module_id_set_) { const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction) { + continue; + } lanelet::ConstLanelets registered_parents = planner_data_->route_handler_->getPreviousLanelets(registered_lane); for (const auto & ll : registered_parents) { @@ -237,20 +268,23 @@ bool IntersectionModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet return false; } -bool MergeFromPrivateModuleManager::hasSameParentLanelet(const lanelet::ConstLanelet & lane) const +bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWith( + const lanelet::ConstLanelet & lane, const size_t module_id, + const std::string & turn_direction) const { + const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(module_id); + const auto turn_direction_registered = registered_lane.attributeOr("turn_direction", "else"); + if (turn_direction_registered != turn_direction) { + return false; + } + lanelet::ConstLanelets registered_parents = + planner_data_->route_handler_->getPreviousLanelets(registered_lane); lanelet::ConstLanelets parents = planner_data_->route_handler_->getPreviousLanelets(lane); - - for (const auto & id : registered_module_id_set_) { - const auto registered_lane = planner_data_->route_handler_->getLaneletsFromId(id); - lanelet::ConstLanelets registered_parents = - planner_data_->route_handler_->getPreviousLanelets(registered_lane); - for (const auto & ll : registered_parents) { - auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); - neighbor_lanes.push_back(ll); - if (hasSameLanelet(parents, neighbor_lanes)) { - return true; - } + for (const auto & ll : registered_parents) { + auto neighbor_lanes = planner_data_->route_handler_->getLaneChangeableNeighbors(ll); + neighbor_lanes.push_back(ll); + if (hasSameLanelet(parents, neighbor_lanes)) { + return true; } } return false; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 0096b922ea75f..1dad05f96fb22 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -96,16 +96,19 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * /* get detection area*/ /* dynamically change detection area based on tl_arrow_solid_on */ - [[maybe_unused]] const bool has_tl = util::hasAssociatedTrafficLight(assigned_lanelet); const bool tl_arrow_solid_on = util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - tl_arrow_solid_on); - const std::vector detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); + if ( + !intersection_lanelets_.has_value() || + intersection_lanelets_.value().tl_arrow_solid_on != tl_arrow_solid_on) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + tl_arrow_solid_on); + } + const auto & detection_lanelets = intersection_lanelets_.value().attention; + const auto & adjacent_lanelets = intersection_lanelets_.value().adjacent; + const auto & detection_area = intersection_lanelets_.value().attention_area; + const auto & conflicting_area = intersection_lanelets_.value().conflicting_area; debug_data_.detection_area = detection_area; /* get intersection area */ @@ -115,25 +118,34 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * debug_data_.intersection_area = toGeomPoly(intersection_area_2d); } - /* get adjacent lanelets */ - const auto adjacent_lanelets = - util::extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); - debug_data_.adjacent_area = util::getPolygon3dFromLanelets(adjacent_lanelets); - - /* set stop lines for base_link */ - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - planner_param_.use_stuck_stopline, path, *path, logger_.get_child("util"), clock_); - if (!stuck_line_idx_opt.has_value()) { - // returns here if path is not intersecting with conflicting areas - RCLCPP_DEBUG_SKIPFIRST_THROTTLE( - logger_, *clock_, 1000 /* ms */, "setStopLineIdx for stuck line fail"); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); setDistance(std::numeric_limits::lowest()); return false; } - const auto stuck_line_idx = stuck_line_idx_opt.value(); + + const auto stuck_line_idx_opt = util::generateStuckStopLine( + lane_id_, conflicting_area, planner_data_, planner_param_.stop_line_margin, + planner_param_.use_stuck_stopline, path, path_ip, interval, lane_interval_ip_opt.value(), + logger_.get_child("util")); + + /* set stop lines for base_link */ + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); /* calc closest index */ const auto closest_idx_opt = @@ -149,14 +161,17 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * const size_t closest_idx = closest_idx_opt.get(); if (stop_lines_idx_opt.has_value()) { - const auto stop_line_idx = stop_lines_idx_opt.value().stop_line; + const auto stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; const auto pass_judge_line_idx = stop_lines_idx_opt.value().pass_judge_line; const bool is_over_pass_judge_line = util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); /* if ego is over the pass judge line before collision is detected, keep going */ - if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + const double current_velocity = planner_data_->current_velocity->twist.linear.x; + if ( + is_over_pass_judge_line && is_go_out_ && + current_velocity > planner_param_.keep_detection_vel_thr) { RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -166,6 +181,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return true; } } + /* collision checking */ bool is_entry_prohibited = false; @@ -189,29 +205,40 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * closest_idx, stuck_vehicle_detect_area, time_delay); /* calculate final stop lines */ - int stop_line_idx_final = - stop_lines_idx_opt.has_value() ? stop_lines_idx_opt.value().stop_line : -1; + std::optional stop_line_idx = std::nullopt; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck || has_collision) { + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; + } else if (is_stuck && stuck_line_idx_opt.has_value()) { is_entry_prohibited = true; const double dist_stuck_stopline = motion_utils::calcSignedArcLength( - path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points, path->points.at(stuck_line_idx_opt.value()).point.pose.position, path->points.at(closest_idx).point.pose.position); const bool is_over_stuck_stopline = - util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx) && + util::isOverTargetIndex(*path, closest_idx, current_pose, stuck_line_idx_opt.value()) && dist_stuck_stopline > planner_param_.stop_overshoot_margin; - if (is_stuck && !is_over_stuck_stopline) { - stop_line_idx_final = stuck_line_idx; - } else if ( - ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { - stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + if (!is_over_stuck_stopline) { + stop_line_idx = stuck_line_idx_opt.value(); + } else if (is_over_stuck_stopline && stop_lines_idx_opt.has_value()) { + stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; } + } else if (has_collision) { + is_entry_prohibited = true; + stop_line_idx = stop_lines_idx_opt.has_value() + ? std::make_optional(stop_lines_idx_opt.value().collision_stop_line) + : std::nullopt; } - if (stop_line_idx_final == -1) { + state_machine_.setStateWithMarginTime( + is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, + logger_.get_child("state_machine"), *clock_); + setSafe(state_machine_.getState() == StateMachine::State::GO); + + if (!stop_line_idx.has_value()) { RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); RCLCPP_DEBUG(logger_, "===== plan end ====="); setSafe(true); @@ -219,30 +246,25 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } - state_machine_.setStateWithMarginTime( - is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, - logger_.get_child("state_machine"), *clock_); - - setSafe(state_machine_.getState() == StateMachine::State::GO); setDistance(motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); + path->points.at(stop_line_idx.value()).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block is_go_out_ = false; constexpr double v = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx_final, v, path); + planning_utils::setVelocityFromIndex(stop_line_idx.value(), v, path); debug_data_.stop_required = true; const double base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; debug_data_.stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx_final, base_link2front, *path); + planning_utils::getAheadPose(stop_line_idx.value(), base_link2front, *path); // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx_final).point.pose; + stop_factor.stop_pose = path->points.at(stop_line_idx.value()).point.pose; const auto stop_factor_conflict = planning_utils::toRosPoints(debug_data_.conflicting_targets); const auto stop_factor_stuck = planning_utils::toRosPoints(debug_data_.stuck_targets); @@ -250,7 +272,7 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * planning_utils::concatVector(stop_factor_conflict, stop_factor_stuck); planning_utils::appendStopReason(stop_factor, stop_reason); - const auto & stop_pose = path->points.at(stop_line_idx_final).point.pose; + const auto & stop_pose = path->points.at(stop_line_idx.value()).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); } diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp index d6084a5d8ce92..a395fc00d5153 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp @@ -64,32 +64,42 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* get detection area */ - auto && [detection_lanelets, conflicting_lanelets] = util::getObjectiveLanelets( - lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, - false /* tl_arrow_solid on does not matter here*/); - if (detection_lanelets.empty()) { - RCLCPP_DEBUG(logger_, "no detection area. skip computation."); - return true; + if (!intersection_lanelets_.has_value()) { + intersection_lanelets_ = util::getObjectiveLanelets( + lanelet_map_ptr, routing_graph_ptr, lane_id_, planner_param_.detection_area_length, + false /* tl_arrow_solid on does not matter here*/); } - const auto detection_area = - util::getPolygon3dFromLanelets(detection_lanelets, planner_param_.detection_area_length); - const std::vector conflicting_area = - util::getPolygon3dFromLanelets(conflicting_lanelets); - debug_data_.detection_area = detection_area; + const auto & detection_area = intersection_lanelets_.value().attention_area; /* set stop-line and stop-judgement-line for base_link */ - const auto private_path = - extractPathNearExitOfPrivateRoad(*path, planner_data_->vehicle_info_.vehicle_length_m); - const auto [stuck_line_idx_opt, stop_lines_idx_opt] = util::generateStopLine( - lane_id_, detection_area, conflicting_area, planner_data_, planner_param_.stop_line_margin, - false /* same */, path, *path, logger_.get_child("util"), clock_); + /* spline interpolation */ + constexpr double interval = 0.2; + autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; + if (!splineInterpolate(*path, interval, path_ip, logger_)) { + RCLCPP_DEBUG_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "splineInterpolate failed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id_); + if (!lane_interval_ip_opt.has_value()) { + RCLCPP_WARN(logger_, "Path has no interval on intersection lane %ld", lane_id_); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const auto stop_lines_idx_opt = util::generateStopLine( + lane_id_, detection_area, planner_data_, planner_param_.stop_line_margin, path, path_ip, + interval, lane_interval_ip_opt.value(), logger_.get_child("util")); if (!stop_lines_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines_idx.stop_line; + const size_t stop_line_idx = stop_lines_idx_opt.value().collision_stop_line; if (stop_line_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; @@ -98,8 +108,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR debug_data_.virtual_wall_pose = planning_utils::getAheadPose( stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; - const size_t first_inside_lane_idx = stop_lines_idx.first_inside_lane; - debug_data_.first_collision_point = path->points.at(first_inside_lane_idx).point.pose.position; /* set stop speed */ if (state_machine_.getState() == StateMachine::State::STOP) { @@ -109,7 +117,6 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = debug_data_.stop_point_pose; - stop_factor.stop_factor_points.emplace_back(debug_data_.first_collision_point); planning_utils::appendStopReason(stop_factor, stop_reason); const auto & stop_pose = path->points.at(stop_line_idx).point.pose; velocity_factor_.set( diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index c047c7814e018..e4ae13c6ce7ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -141,13 +141,12 @@ std::optional getFirstPointInsidePolygons( return first_idx_inside_lanelet; } -std::pair, std::optional> generateStopLine( +std::optional generateStopLine( const int lane_id, const std::vector & detection_areas, - const std::vector & conflicting_areas, const std::shared_ptr & planner_data, const double stop_line_margin, - const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, - const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) + autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -158,81 +157,15 @@ std::pair, std::optional> generateStopLine( const double pass_judge_line_dist = planning_utils::calcJudgeLineDistWithJerkLimit( current_vel, current_acc, max_acc, max_jerk, delay_response_time); - // first inside lane idx - const auto first_inside_lane_it = - std::find_if(original_path->points.begin(), original_path->points.end(), [&](const auto & p) { - return std::find(p.lane_ids.begin(), p.lane_ids.end(), lane_id) != p.lane_ids.end(); - }); - if (first_inside_lane_it == original_path->points.end()) { - RCLCPP_ERROR(logger, "No points on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const size_t first_inside_lane_idx = - std::distance(original_path->points.begin(), first_inside_lane_it); - - /* spline interpolation */ - constexpr double interval = 0.2; - autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; - if (!splineInterpolate(target_path, interval, path_ip, logger)) { - return {std::nullopt, std::nullopt}; - } - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); const int pass_judge_idx_dist = std::ceil(pass_judge_line_dist / interval); - const auto lane_interval_ip_opt = util::findLaneIdInterval(path_ip, lane_id); - if (!lane_interval_ip_opt.has_value()) { - RCLCPP_WARN(logger, "Path has no interval on intersection lane %d", lane_id); - return {std::nullopt, std::nullopt}; - } - const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip_opt.value(); - - /* generate stuck stop line */ - size_t stuck_stop_line_idx_ip = 0; - if (use_stuck_stopline) { - // the first point in intersection lane - stuck_stop_line_idx_ip = lane_interval_ip_start; - } else { - const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( - path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); - if (!stuck_stop_line_idx_ip_opt.has_value()) { - RCLCPP_DEBUG( - logger, - "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " - "%ld, end = %ld", - lane_interval_ip_start, lane_interval_ip_end); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); - } - - size_t stuck_stop_line_idx = 0; - { - /* insert stuck stop line */ - const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - - base2front_idx_dist, - 0)); - const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; - const auto duplicate_idx_opt = - util::getDuplicatedPointIdx(*original_path, insert_point.position); - if (duplicate_idx_opt.has_value()) { - stuck_stop_line_idx = duplicate_idx_opt.value(); - } else { - const auto insert_idx_opt = util::insertPoint(insert_point, original_path); - if (!insert_idx_opt.has_value()) { - RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); - return {std::nullopt, std::nullopt}; - } - stuck_stop_line_idx = insert_idx_opt.value(); - } - } + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; /* generate stop points */ util::StopLineIdx idxs; - idxs.first_inside_lane = first_inside_lane_idx; // If a stop_line tag is defined on lanelet_map, use it. // else generate a stop_line behind the intersection of path and detection area @@ -251,7 +184,7 @@ std::pair, std::optional> generateStopLine( if (!first_inside_detection_idx_ip_opt.has_value()) { RCLCPP_DEBUG( logger, "Path is not intersecting with detection_area, not generating stop_line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } const auto first_inside_detection_idx_ip = first_inside_detection_idx_ip_opt.value(); @@ -262,7 +195,7 @@ std::pair, std::optional> generateStopLine( } if (stop_idx_ip == 0) { RCLCPP_DEBUG(logger, "stop line is at path[0], ignore planning\n===== plan end ====="); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } { @@ -271,19 +204,19 @@ std::pair, std::optional> generateStopLine( const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); if (duplicate_idx_opt.has_value()) { - idxs.stop_line = duplicate_idx_opt.value(); + idxs.collision_stop_line = duplicate_idx_opt.value(); } else { const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed for stop line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } - idxs.stop_line = insert_idx_opt.value(); + idxs.collision_stop_line = insert_idx_opt.value(); } } const bool has_prior_stopline = std::any_of( - original_path->points.begin(), original_path->points.begin() + idxs.stop_line, + original_path->points.begin(), original_path->points.begin() + idxs.collision_stop_line, [](const auto & p) { return std::fabs(p.point.longitudinal_velocity_mps) < 0.1; }); /* insert judge point */ @@ -292,7 +225,7 @@ std::pair, std::optional> generateStopLine( std::max(static_cast(stop_idx_ip) - pass_judge_idx_dist, 0))); /* if another stop point exist before intersection stop_line, disable judge_line. */ if (has_prior_stopline || pass_judge_idx_ip == stop_idx_ip) { - idxs.pass_judge_line = idxs.stop_line; + idxs.pass_judge_line = idxs.collision_stop_line; } else { const auto & insert_point = path_ip.points.at(pass_judge_idx_ip).point.pose; const auto duplicate_idx_opt = @@ -303,24 +236,68 @@ std::pair, std::optional> generateStopLine( const auto insert_idx_opt = util::insertPoint(insert_point, original_path); if (!insert_idx_opt.has_value()) { RCLCPP_WARN(logger, "insertPoint failed to pass judge line"); - return {stuck_stop_line_idx, std::nullopt}; + return std::nullopt; } idxs.pass_judge_line = insert_idx_opt.value(); - idxs.stop_line = std::min(idxs.stop_line + 1, original_path->points.size() - 1); - if (stuck_stop_line_idx >= idxs.pass_judge_line) { - stuck_stop_line_idx = - std::min(stuck_stop_line_idx + 1, original_path->points.size() - 1); - } + idxs.collision_stop_line = + std::min(idxs.collision_stop_line + 1, original_path->points.size() - 1); } } RCLCPP_DEBUG( logger, "generateStopLine() : stop_idx = %ld, pass_judge_idx = %ld" - ", stuck_stop_idx = %ld, has_prior_stopline = %d", - idxs.stop_line, idxs.pass_judge_line, stuck_stop_line_idx, has_prior_stopline); + ", has_prior_stopline = %d", + idxs.collision_stop_line, idxs.pass_judge_line, has_prior_stopline); + + return std::make_optional(idxs); +} + +std::optional generateStuckStopLine( + const int lane_id, const std::vector & conflicting_areas, + const std::shared_ptr & planner_data, const double stop_line_margin, + const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path_ip, const double interval, + const std::pair lane_interval_ip, const rclcpp::Logger logger) +{ + size_t stuck_stop_line_idx_ip = 0; + const auto [lane_interval_ip_start, lane_interval_ip_end] = lane_interval_ip; + if (use_stuck_stopline) { + stuck_stop_line_idx_ip = lane_interval_ip_start; + } else { + const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( + path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); + if (!stuck_stop_line_idx_ip_opt.has_value()) { + RCLCPP_DEBUG( + logger, + "Path is not intersecting with conflicting area, not generating stuck_stop_line. start = " + "%ld, end = %ld", + lane_interval_ip_start, lane_interval_ip_end); + return std::nullopt; + } + stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); + } - return {stuck_stop_line_idx, std::make_optional(idxs)}; + const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / interval); + const int base2front_idx_dist = + std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / interval); + size_t stuck_stop_line_idx = 0; + const size_t insert_idx_ip = static_cast(std::max( + static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist, + 0)); + const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; + const auto duplicate_idx_opt = util::getDuplicatedPointIdx(*original_path, insert_point.position); + if (duplicate_idx_opt.has_value()) { + stuck_stop_line_idx = duplicate_idx_opt.value(); + return std::make_optional(stuck_stop_line_idx); + } else { + const auto insert_idx_opt = util::insertPoint(insert_point, original_path); + if (!insert_idx_opt.has_value()) { + RCLCPP_WARN(logger, "insertPoint failed for stuck stop line"); + return std::nullopt; + } + return std::make_optional(insert_idx_opt.value()); + } } bool getStopLineIndexFromMap( @@ -387,7 +364,7 @@ bool getStopLineIndexFromMap( return true; } -std::tuple getObjectiveLanelets( +IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id, const double detection_area_length, const bool tl_arrow_solid_on) { @@ -461,9 +438,9 @@ std::tuple getObjectiveLanelets( // get possible lanelet path that reaches conflicting_lane longer than given length // if traffic light arrow is active, this process is unnecessary + lanelet::ConstLanelets detection_and_preceding_lanelets; if (!tl_arrow_solid_on) { const double length = detection_area_length; - lanelet::ConstLanelets detection_and_preceding_lanelets; std::set detection_ids; for (const auto & ll : detection_lanelets) { // Preceding lanes does not include detection_lane so add them at the end @@ -480,10 +457,22 @@ std::tuple getObjectiveLanelets( } } } - return {std::move(detection_and_preceding_lanelets), std::move(conflicting_ex_ego_lanelets)}; - } else { - return {std::move(detection_lanelets), std::move(conflicting_ex_ego_lanelets)}; } + + IntersectionLanelets result; + if (!tl_arrow_solid_on) { + result.attention = std::move(detection_and_preceding_lanelets); + } else { + result.attention = std::move(detection_lanelets); + } + result.conflicting = std::move(conflicting_ex_ego_lanelets); + result.adjacent = + extendedAdjacentDirectionLanes(lanelet_map_ptr, routing_graph_ptr, assigned_lanelet); + // compoundPolygon3d + result.attention_area = getPolygon3dFromLanelets(result.attention); + result.conflicting_area = getPolygon3dFromLanelets(result.conflicting); + result.adjacent_area = getPolygon3dFromLanelets(result.adjacent); + return result; } std::vector getPolygon3dFromLanelets( @@ -574,7 +563,7 @@ static std::vector getAllAdjacentLanelets( } lanelet::ConstLanelets extendedAdjacentDirectionLanes( - const lanelet::LaneletMapPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, + lanelet::LaneletMapConstPtr map, const lanelet::routing::RoutingGraphPtr routing_graph, lanelet::ConstLanelet lane) { // some of the intersections are not well-formed, and "adjacent" turning diff --git a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp index 363cb191d3405..45ac7025e89a5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/traffic_light/manager.cpp @@ -125,7 +125,7 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); const auto module_id = lane_id; - if (!isModuleRegisteredFromRegElement(module_id)) { + if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) { registerModule(std::make_shared( module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, logger_.get_child("traffic_light_module"), clock_)); @@ -143,10 +143,9 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [this, lanelet_id_set]( - [[maybe_unused]] const std::shared_ptr & scene_module) { + return [this, lanelet_id_set](const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { - if (isModuleRegisteredFromRegElement(id)) { + if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) { return false; } } @@ -154,7 +153,25 @@ TrafficLightModuleManager::getModuleExpiredFunction( }; } -bool TrafficLightModuleManager::isModuleRegisteredFromRegElement(const lanelet::Id & id) const +bool TrafficLightModuleManager::isModuleRegisteredFromRegElement( + const lanelet::Id & id, const size_t module_id) const +{ + const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); + + const auto registered_lane = + planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id); + for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { + for (const auto & element : lane.regulatoryElementsAs()) { + if (hasSameTrafficLight(element, registered_element)) { + return true; + } + } + } + return false; +} + +bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule( + const lanelet::Id & id) const { const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); diff --git a/planning/freespace_planner/package.xml b/planning/freespace_planner/package.xml index 614049e1e5222..62e3cbe73fa2b 100644 --- a/planning/freespace_planner/package.xml +++ b/planning/freespace_planner/package.xml @@ -4,10 +4,12 @@ freespace_planner 0.1.0 The freespace_planner package + Kosuke Takeuchi Takamasa Horibe + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 - Kenji Miyake Akihito OHSATO Tomohito ANDO diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp index b59888fa52281..f9ecdbc826d5d 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp @@ -111,6 +111,7 @@ class AbstractPlanningAlgorithm const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0; virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const; const PlannerWaypoints & getWaypoints() const { return waypoints_; } + virtual ~AbstractPlanningAlgorithm() {} protected: diff --git a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp index 9ccec85bfc185..6f1810fabec88 100644 --- a/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp +++ b/planning/freespace_planning_algorithms/include/freespace_planning_algorithms/astar_search.hpp @@ -144,6 +144,7 @@ class AstarSearch : public AbstractPlanningAlgorithm bool setGoalNode(); double estimateCost(const geometry_msgs::msg::Pose & pose) const; bool isGoal(const AstarNode & node) const; + geometry_msgs::msg::Pose node2pose(const AstarNode & node) const; AstarNode * getNodeRef(const IndexXYT & index) { diff --git a/planning/freespace_planning_algorithms/package.xml b/planning/freespace_planning_algorithms/package.xml index b8740d435897c..4fc302d58de65 100644 --- a/planning/freespace_planning_algorithms/package.xml +++ b/planning/freespace_planning_algorithms/package.xml @@ -4,7 +4,10 @@ freespace_planning_algorithms 0.1.0 The freespace_planning_algorithms package + Kosuke Takeuchi Takamasa Horibe + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 Akihito Ohsato @@ -16,6 +19,7 @@ geometry_msgs nav_msgs + nlohmann-json-dev rosbag2_cpp std_msgs tf2 diff --git a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp index 143f147b79eee..2b5814ee2a111 100644 --- a/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp +++ b/planning/freespace_planning_algorithms/src/abstract_algorithm.cpp @@ -89,6 +89,9 @@ geometry_msgs::msg::Pose local2global( double PlannerWaypoints::compute_length() const { + if (waypoints.empty()) { + std::runtime_error("cannot compute cost because waypoint has size 0"); + } double total_cost = 0.0; for (size_t i = 0; i < waypoints.size() - 1; ++i) { const auto pose_a = waypoints.at(i); diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 8d419c3daa13f..5686d7eee26e0 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -61,18 +61,6 @@ geometry_msgs::msg::Pose calcRelativePose( return transformed.pose; } -geometry_msgs::msg::Pose node2pose(const AstarNode & node) -{ - geometry_msgs::msg::Pose pose_local; - - pose_local.position.x = node.x; - pose_local.position.y = node.y; - pose_local.position.z = 0; - pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); - - return pose_local; -} - AstarSearch::TransitionTable createTransitionTable( const double minimum_turning_radius, const double maximum_turning_radius, const int turning_radius_size, const double theta_size, const bool use_back) @@ -356,4 +344,16 @@ bool AstarSearch::isGoal(const AstarNode & node) const return true; } +geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const +{ + geometry_msgs::msg::Pose pose_local; + + pose_local.position.x = node.x; + pose_local.position.y = node.y; + pose_local.position.z = goal_pose_.position.z; + pose_local.orientation = tier4_autoware_utils::createQuaternionFromYaw(node.theta); + + return pose_local; +} + } // namespace freespace_planning_algorithms diff --git a/planning/freespace_planning_algorithms/src/rrtstar.cpp b/planning/freespace_planning_algorithms/src/rrtstar.cpp index d710b74111b1a..f03dc81faba05 100644 --- a/planning/freespace_planning_algorithms/src/rrtstar.cpp +++ b/planning/freespace_planning_algorithms/src/rrtstar.cpp @@ -141,7 +141,7 @@ void RRTStar::setRRTPath(const std::vector & waypoints) geometry_msgs::msg::Pose pose_local; pose_local.position.x = pt.x; pose_local.position.y = pt.y; - pose_local.position.z = 0.0; + pose_local.position.z = goal_pose_.position.z; tf2::Quaternion quat; quat.setRPY(0, 0, pt.yaw); tf2::convert(quat, pose_local.orientation); diff --git a/planning/freespace_planning_algorithms/test/debug_plot.py b/planning/freespace_planning_algorithms/test/debug_plot.py index f6c77bec52466..7d6277a2756c6 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot.py +++ b/planning/freespace_planning_algorithms/test/debug_plot.py @@ -202,7 +202,6 @@ def create_concate_png(src_list, dest, is_horizontal): algo_name = algo_names[i] algo_pngs = [] for j in range(n_case): - fig, ax = plt.subplots() result_dir = dir_name_table[(algo_name, j)] diff --git a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py index f263e08bf968c..3fa6d152eba11 100755 --- a/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py +++ b/planning/freespace_planning_algorithms/test/debug_plot_rrtstar_core.py @@ -97,7 +97,6 @@ def visualize(self): if __name__ == "__main__": - with open("/tmp/rrt_result.txt", "r") as f: dict_data = json.load(f) tree = Tree.from_dict(dict_data) diff --git a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp index 08c23c24f73d6..8ea9e0b4af8b5 100644 --- a/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp +++ b/planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp @@ -270,8 +270,6 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) for (size_t i = 0; i < goal_poses.size(); ++i) { const auto goal_pose = goal_poses.at(i); - bool success_local = true; - algo->setMap(costmap_msg); double msec; double cost; @@ -284,7 +282,9 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) for (size_t j = 0; j < N_mc; j++) { const rclcpp::Time begin = clock.now(); if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { - success_local = false; + success_all = false; + std::cout << "plan fail" << std::endl; + continue; } const rclcpp::Time now = clock.now(); time_sum += (now - begin).seconds() * 1000.0; @@ -295,19 +295,18 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false) } else { const rclcpp::Time begin = clock.now(); - success_local = algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose)); + if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) { + success_all = false; + std::cout << "plan fail" << std::endl; + continue; + } const rclcpp::Time now = clock.now(); msec = (now - begin).seconds() * 1000.0; cost = algo->getWaypoints().compute_length(); } - if (success_local) { - std::cout << "plan success : " << msec << "[msec]" - << ", solution cost : " << cost << std::endl; - } else { - success_all = false; - std::cout << "plan fail : " << msec << "[msec]" << std::endl; - } + std::cout << "plan success : " << msec << "[msec]" + << ", solution cost : " << cost << std::endl; const auto result = algo->getWaypoints(); geometry_msgs::msg::PoseArray trajectory; for (const auto & pose : result.waypoints) { diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp index 262473565daab..bb656a30f3a4d 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/motion_velocity_smoother_node.hpp @@ -208,6 +208,10 @@ class MotionVelocitySmootherNode : public rclcpp::Node Trajectory toTrajectoryMsg( const TrajectoryPoints & points, const std_msgs::msg::Header * header = nullptr) const; + TrajectoryPoint calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const; + TrajectoryPoint calcProjectedTrajectoryPointFromEgo(const TrajectoryPoints & trajectory) const; + // parameter handling void initCommonParam(); diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index c3d7af26e4cf8..9cddba5ca500a 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -77,10 +77,10 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase [[maybe_unused]] const double nearest_dist_threshold, [[maybe_unused]] const double nearest_yaw_threshold) const override; - boost::optional applyLateralAccelerationFilter( + TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, - [[maybe_unused]] const bool enable_smooth_limit) const override; + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling = true, const double input_points_interval = 1.0) const override; void setParam(const Param & param); Param getParam() const; diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp index 1b75981bc823b..ea85f8dc78615 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/smoother/smoother_base.hpp @@ -71,12 +71,14 @@ class SmootherBase const TrajectoryPoints & input, const double v0, const geometry_msgs::msg::Pose & current_pose, const double nearest_dist_threshold, const double nearest_yaw_threshold) const = 0; - virtual boost::optional applyLateralAccelerationFilter( + virtual TrajectoryPoints applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0 = 0.0, - [[maybe_unused]] const double a0 = 0.0, - [[maybe_unused]] const bool enable_smooth_limit = false) const; + [[maybe_unused]] const double a0 = 0.0, [[maybe_unused]] const bool enable_smooth_limit = false, + const bool use_resampling = true, const double input_points_interval = 1.0) const; - boost::optional applySteeringRateLimit(const TrajectoryPoints & input) const; + TrajectoryPoints applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling = true, + const double input_points_interval = 1.0) const; double getMaxAccel() const; double getMinDecel() const; diff --git a/planning/motion_velocity_smoother/package.xml b/planning/motion_velocity_smoother/package.xml index 2550abdd03005..e5e25a55c4e19 100644 --- a/planning/motion_velocity_smoother/package.xml +++ b/planning/motion_velocity_smoother/package.xml @@ -6,6 +6,9 @@ The motion_velocity_smoother package Fumiya Watanabe + Takamasa Horibe + Yutaka Shimizu + Makoto Kurihara Apache License 2.0 Takamasa Horibe diff --git a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp index fdba04a625661..9dc88115a85d8 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -399,12 +399,7 @@ void MotionVelocitySmootherNode::onCurrentTrajectory(const Trajectory::ConstShar // calculate prev closest point if (!prev_output_.empty()) { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); - current_closest_point_from_prev_output_ = closest_point; + current_closest_point_from_prev_output_ = calcProjectedTrajectoryPointFromEgo(prev_output_); } // calculate distance to insert external velocity limit @@ -528,30 +523,26 @@ bool MotionVelocitySmootherNode::smoothVelocity( const TrajectoryPoints & input, const size_t input_closest, TrajectoryPoints & traj_smoothed) const { + if (input.empty()) { + return false; // cannot apply smoothing + } + // Calculate initial motion for smoothing const auto [initial_motion, type] = calcInitialMotion(input, input_closest); // Lateral acceleration limit - const auto traj_lateral_acc_filtered = - smoother_->applyLateralAccelerationFilter(input, initial_motion.vel, initial_motion.acc, true); - if (!traj_lateral_acc_filtered) { - RCLCPP_ERROR(get_logger(), "Fail to do traj_lateral_acc_filtered"); - - return false; - } + constexpr bool enable_smooth_limit = true; + constexpr bool use_resampling = true; + const auto traj_lateral_acc_filtered = smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling); - // Steering angle rate limit + // Steering angle rate limit (Note: set use_resample = false since it is resampled above) const auto traj_steering_rate_limited = - smoother_->applySteeringRateLimit(*traj_lateral_acc_filtered); - if (!traj_steering_rate_limited) { - RCLCPP_ERROR(get_logger(), "Fail to do traj_steering_rate_limited"); - - return false; - } + smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false); // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( - *traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, + traj_steering_rate_limited, current_odometry_ptr_->twist.twist.linear.x, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, node_param_.ego_nearest_yaw_threshold); @@ -598,7 +589,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( RCLCPP_DEBUG(get_logger(), "smoothVelocity : traj_smoothed.size() = %lu", traj_smoothed.size()); if (publish_debug_trajs_) { { - auto tmp = *traj_lateral_acc_filtered; + auto tmp = traj_lateral_acc_filtered; if (is_reverse_) flipVelocity(tmp); pub_trajectory_latacc_filtered_->publish(toTrajectoryMsg(tmp)); } @@ -608,7 +599,7 @@ bool MotionVelocitySmootherNode::smoothVelocity( pub_trajectory_resampled_->publish(toTrajectoryMsg(tmp)); } { - auto tmp = *traj_steering_rate_limited; + auto tmp = traj_steering_rate_limited; if (is_reverse_) flipVelocity(tmp); pub_trajectory_steering_rate_limited_->publish(toTrajectoryMsg(tmp)); } @@ -884,11 +875,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( const TrajectoryPoints & trajectory, const Pose & current_pose, const rclcpp::Publisher::SharedPtr pub) const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, current_pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = - trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, current_pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPoint(trajectory, current_pose); Float32Stamped vel_data{}; vel_data.stamp = this->now(); @@ -898,11 +885,7 @@ void MotionVelocitySmootherNode::publishClosestVelocity( void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & trajectory) { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - trajectory, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - trajectory, current_odometry_ptr_->pose.pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPointFromEgo(trajectory); auto publishFloat = [=](const double data, const auto pub) { Float32Stamped msg{}; @@ -938,12 +921,7 @@ void MotionVelocitySmootherNode::publishClosestState(const TrajectoryPoints & tr void MotionVelocitySmootherNode::updatePrevValues(const TrajectoryPoints & final_result) { prev_output_ = final_result; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - final_result, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - final_result, current_odometry_ptr_->pose.pose, current_seg_idx); - prev_closest_point_ = closest_point; + prev_closest_point_ = calcProjectedTrajectoryPointFromEgo(final_result); } MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorithmType( @@ -968,11 +946,7 @@ MotionVelocitySmootherNode::AlgorithmType MotionVelocitySmootherNode::getAlgorit double MotionVelocitySmootherNode::calcTravelDistance() const { - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_output_, current_odometry_ptr_->pose.pose, node_param_.ego_nearest_dist_threshold, - node_param_.ego_nearest_yaw_threshold); - const auto closest_point = trajectory_utils::calcInterpolatedTrajectoryPoint( - prev_output_, current_odometry_ptr_->pose.pose, current_seg_idx); + const auto closest_point = calcProjectedTrajectoryPointFromEgo(prev_output_); if (prev_closest_point_) { const double travel_dist = @@ -1027,6 +1001,21 @@ void MotionVelocitySmootherNode::publishStopWatchTime() debug_calculation_time_->publish(calculation_time_data); } +TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPoint( + const TrajectoryPoints & trajectory, const Pose & pose) const +{ + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory, pose, node_param_.ego_nearest_dist_threshold, + node_param_.ego_nearest_yaw_threshold); + return trajectory_utils::calcInterpolatedTrajectoryPoint(trajectory, pose, current_seg_idx); +} + +TrajectoryPoint MotionVelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( + const TrajectoryPoints & trajectory) const +{ + return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); +} + } // namespace motion_velocity_smoother #include "rclcpp_components/register_node_macro.hpp" diff --git a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index d9b06bcd4da72..3b013b77e4b01 100644 --- a/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/motion_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -277,29 +277,33 @@ TrajectoryPoints AnalyticalJerkConstrainedSmoother::resampleTrajectory( return output; } -boost::optional AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( +TrajectoryPoints AnalyticalJerkConstrainedSmoother::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional(input); // cannot calculate lateral acc. do nothing. + return input; // cannot calculate lateral acc. do nothing. } // Interpolate with constant interval distance for lateral acceleration calculation. - constexpr double points_interval = 0.1; // [m] - std::vector out_arclength; - const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); - for (double s = 0; s < in_arclength.back(); s += points_interval) { - out_arclength.push_back(s); + const double points_interval = use_resampling ? 0.1 : input_points_interval; // [m] + + TrajectoryPoints output; + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const std::vector in_arclength = trajectory_utils::calcArclengthArray(input); + for (double s = 0; s < in_arclength.back(); s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points const size_t idx_dist = diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index ca9af83a86864..b0688fa698f1f 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -69,31 +69,36 @@ double SmootherBase::getMaxJerk() const { return base_param_.max_jerk; } double SmootherBase::getMinJerk() const { return base_param_.min_jerk; } -boost::optional SmootherBase::applyLateralAccelerationFilter( +TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( const TrajectoryPoints & input, [[maybe_unused]] const double v0, - [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit) const + [[maybe_unused]] const double a0, [[maybe_unused]] const bool enable_smooth_limit, + const bool use_resampling, const double input_points_interval) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional(input); // cannot calculate lateral acc. do nothing. + return input; // cannot calculate lateral acc. do nothing. } + constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points + // Interpolate with constant interval distance for lateral acceleration calculation. - constexpr double points_interval = 0.1; // [m] - std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); - for (double s = 0; s < traj_length; s += points_interval) { - out_arclength.push_back(s); + TrajectoryPoints output; + const double points_interval = + use_resampling ? base_param_.sample_ds : input_points_interval; // [m] + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. - constexpr double curvature_calc_dist = 5.0; // [m] calc curvature with 5m away points const size_t idx_dist = static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 1)); @@ -135,30 +140,34 @@ boost::optional SmootherBase::applyLateralAccelerationFilter( return output; } -boost::optional SmootherBase::applySteeringRateLimit( - const TrajectoryPoints & input) const +TrajectoryPoints SmootherBase::applySteeringRateLimit( + const TrajectoryPoints & input, const bool use_resampling, + const double input_points_interval) const { - if (input.empty()) { - return boost::none; - } - if (input.size() < 3) { - return boost::optional( - input); // cannot calculate the desired velocity. do nothing. + return input; // cannot calculate the desired velocity. do nothing. } + // Interpolate with constant interval distance for lateral acceleration calculation. - std::vector out_arclength; - const auto traj_length = motion_utils::calcArcLength(input); - for (double s = 0; s < traj_length; s += base_param_.sample_ds) { - out_arclength.push_back(s); + const double points_interval = use_resampling ? base_param_.sample_ds : input_points_interval; + TrajectoryPoints output; + // since the resampling takes a long time, omit the resampling when it is not requested + if (use_resampling) { + std::vector out_arclength; + const auto traj_length = motion_utils::calcArcLength(input); + for (double s = 0; s < traj_length; s += points_interval) { + out_arclength.push_back(s); + } + const auto output_traj = + motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); + output = motion_utils::convertToTrajectoryPointArray(output_traj); + output.back() = input.back(); // keep the final speed. + } else { + output = input; } - const auto output_traj = - motion_utils::resampleTrajectory(motion_utils::convertToTrajectory(input), out_arclength); - auto output = motion_utils::convertToTrajectoryPointArray(output_traj); - output.back() = input.back(); // keep the final speed. - const size_t idx_dist = static_cast(std::max( - static_cast((base_param_.curvature_calculation_distance) / base_param_.sample_ds), 1)); + const size_t idx_dist = static_cast( + std::max(static_cast((base_param_.curvature_calculation_distance) / points_interval), 1)); // Calculate curvature assuming the trajectory points interval is constant const auto curvature_v = trajectory_utils::calcTrajectoryCurvatureFrom3Points(output, idx_dist); @@ -173,14 +182,14 @@ boost::optional SmootherBase::applySteeringRateLimit( const double mean_vel = (output.at(i).longitudinal_velocity_mps + output.at(i + 1).longitudinal_velocity_mps) / 2.0; const double dt = - std::max(base_param_.sample_ds / mean_vel, std::numeric_limits::epsilon()); + std::max(points_interval / mean_vel, std::numeric_limits::epsilon()); const double steering_diff = fabs(output.at(i).front_wheel_angle_rad - output.at(i + 1).front_wheel_angle_rad); const double dt_steering = steering_diff / tier4_autoware_utils::deg2rad(base_param_.max_steering_angle_rate); if (dt_steering > dt) { - const double target_mean_vel = (base_param_.sample_ds / dt_steering); + const double target_mean_vel = (points_interval / dt_steering); for (size_t k = 0; k < 2; k++) { const double temp_vel = output.at(i + k).longitudinal_velocity_mps * (target_mean_vel / mean_vel); diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py b/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py index 55f7b447fc42a..908cc204da2e5 100644 --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py +++ b/planning/obstacle_avoidance_planner/scripts/calclation_time_analyzer.py @@ -79,7 +79,6 @@ def CallbackCalculationCost(self, msg): if __name__ == "__main__": - parser = argparse.ArgumentParser() parser.add_argument("-f", "--functions", type=str, default="solveOsqp") args = parser.parse_args() diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index ceaaeeb865030..75466d4362b13 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -1472,8 +1472,8 @@ void MPTOptimizer::calcVehicleBounds( return; } - SplineInterpolationPoints2d ref_points_spline_interpolation; - ref_points_spline_interpolation.calcSplineCoefficients(points_utils::convertToPoints(ref_points)); + SplineInterpolationPoints2d ref_points_spline_interpolation( + points_utils::convertToPoints(ref_points)); for (size_t p_idx = 0; p_idx < ref_points.size(); ++p_idx) { const auto & ref_point = ref_points.at(p_idx); diff --git a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp index 2f1ba9f3a58ef..30a74c0392a0a 100644 --- a/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils/debug_utils.cpp @@ -445,7 +445,7 @@ visualization_msgs::msg::MarkerArray getBoundsCandidatesLineMarkerArray( visualization_msgs::msg::MarkerArray msg; const std::string ns = "bounds_candidates"; - if (ref_points.empty()) return msg; + if (ref_points.empty() || bounds_candidates.empty()) return msg; auto marker = createDefaultMarker( "map", rclcpp::Clock().now(), ns, 0, visualization_msgs::msg::Marker::LINE_LIST, diff --git a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py index 00833d4e84e83..5e5cfb207a625 100755 --- a/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py +++ b/planning/obstacle_cruise_planner/scripts/trajectory_visualizer.py @@ -36,7 +36,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py index 087325c6cc0a8..d26f6133b0b54 100755 --- a/planning/obstacle_velocity_limiter/script/velocity_visualizer.py +++ b/planning/obstacle_velocity_limiter/script/velocity_visualizer.py @@ -24,7 +24,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/planning_debug_tools/scripts/trajectory_visualizer.py b/planning/planning_debug_tools/scripts/trajectory_visualizer.py index 8845bbb52c056..f4ff9b5587312 100755 --- a/planning/planning_debug_tools/scripts/trajectory_visualizer.py +++ b/planning/planning_debug_tools/scripts/trajectory_visualizer.py @@ -62,7 +62,6 @@ class TrajectoryVisualizer(Node): def __init__(self): - super().__init__("trajectory_visualizer") self.fig = plt.figure() diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py index 517b6eaf9e477..e4bcdeab1232c 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py +++ b/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py @@ -27,7 +27,6 @@ @pytest.mark.launch_test def generate_test_description(): - lanelet2_map_path = os.path.join( get_package_share_directory("static_centerline_optimizer"), "test/data/lanelet2_map.osm" ) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index a270be702fb4c..742f87411b7d8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -44,6 +44,7 @@ #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/imu.hpp" #include "tier4_external_api_msgs/srv/initialize_pose.hpp" #include @@ -82,6 +83,7 @@ using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::TwistStamped; using nav_msgs::msg::Odometry; +using sensor_msgs::msg::Imu; using tier4_external_api_msgs::srv::InitializePose; class DeltaTime @@ -126,6 +128,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_odom_; rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_acc_; + rclcpp::Publisher::SharedPtr pub_imu_; rclcpp::Publisher::SharedPtr pub_control_mode_report_; rclcpp::Publisher::SharedPtr pub_gear_report_; rclcpp::Publisher::SharedPtr pub_turn_indicators_report_; @@ -328,6 +331,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_acceleration(); + /** + * @brief publish imu + */ + void publish_imu(); + /** * @brief publish control_mode report */ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 43a0fe25888c7..321e8c9311f79 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -66,6 +66,7 @@ def launch_setup(context, *args, **kwargs): ("output/twist", "/vehicle/status/velocity_status"), ("output/odometry", "/localization/kinematic_state"), ("output/acceleration", "/localization/acceleration"), + ("output/imu", "/sensing/imu/imu_data"), ("output/steering", "/vehicle/status/steering_status"), ("output/gear_report", "/vehicle/status/gear_status"), ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 1974020d802ab..ca4766f875dbe 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -20,6 +20,7 @@ nav_msgs rclcpp rclcpp_components + sensor_msgs tf2 tf2_geometry_msgs tf2_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 121d9511478a7..cfc58cdaeed63 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -130,6 +130,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); pub_acc_ = create_publisher("output/acceleration", QoS{1}); + pub_imu_ = create_publisher("output/imu", QoS{1}); pub_tf_ = create_publisher("/tf", QoS{1}); /* set param callback */ @@ -298,6 +299,7 @@ void SimplePlanningSimulator::on_timer() publish_velocity(current_velocity_); publish_steering(current_steer_); publish_acceleration(); + publish_imu(); publish_control_mode_report(); publish_gear_report(); @@ -563,6 +565,29 @@ void SimplePlanningSimulator::publish_acceleration() pub_acc_->publish(msg); } +void SimplePlanningSimulator::publish_imu() +{ + using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; + + sensor_msgs::msg::Imu imu; + imu.header.frame_id = "base_link"; + imu.header.stamp = now(); + imu.linear_acceleration.x = vehicle_model_ptr_->getAx(); + constexpr auto COV = 0.001; + imu.linear_acceleration_covariance.at(COV_IDX::X_X) = COV; + imu.linear_acceleration_covariance.at(COV_IDX::Y_Y) = COV; + imu.linear_acceleration_covariance.at(COV_IDX::Z_Z) = COV; + imu.angular_velocity = current_odometry_.twist.twist.angular; + imu.angular_velocity_covariance.at(COV_IDX::X_X) = COV; + imu.angular_velocity_covariance.at(COV_IDX::Y_Y) = COV; + imu.angular_velocity_covariance.at(COV_IDX::Z_Z) = COV; + imu.orientation = current_odometry_.pose.pose.orientation; + imu.orientation_covariance.at(COV_IDX::X_X) = COV; + imu.orientation_covariance.at(COV_IDX::Y_Y) = COV; + imu.orientation_covariance.at(COV_IDX::Z_Z) = COV; + pub_imu_->publish(imu); +} + void SimplePlanningSimulator::publish_control_mode_report() { current_control_mode_.stamp = get_clock()->now(); diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py index a031cc679abaf..ce9667d23bf6c 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/calc_utils.py @@ -71,7 +71,6 @@ def lowpass_filter_scipy(x, sample_rate, fp, fs, g_pass, g_stop): def create_2d_map( x, y, data, color_factor, x_index_list, x_thresh, y_index_list, y_thresh, calibration_method ): - if x.shape != y.shape or y.shape != data.shape: print("Error: the shape of x, y, data must be same") sys.exit() diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py index 9b5cfc9186495..a2d82a460aed0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/new_accel_brake_map_server.py @@ -254,7 +254,6 @@ def view_pedal_accel_graph( calibrated_pedal_list, calibrated_acc_list, ): - fig = plotter.subplot_more(subplot_num) # calibrated map diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py index 6053d787c5e92..5541dc4f95748 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/plotter.py @@ -102,7 +102,7 @@ def imshow(self, data, left, right, ws, bottom, top, hs, num_data_type="float"): origin="lower", ) ys, xs = np.meshgrid(width_range, height_range, indexing="ij") - for (x, y, val) in zip(xs.flatten(), ys.flatten(), data.flatten()): + for x, y, val in zip(xs.flatten(), ys.flatten(), data.flatten()): self.plot_text(x, y, val, num_data_type) plt.colorbar() diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py index 3fc263d990cbb..4d713cf4848e0 100755 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/scripts/view_plot.py @@ -158,7 +158,6 @@ def view_pedal_accel_graph( calibrated_acc_list, scatter_only, ): - fig = plotter.subplot_more(subplot_num) if not scatter_only: @@ -281,7 +280,7 @@ def main(args=None): parser.add_argument( "-m", "--method", - default="None", + default=None, type=str, help="calibration method : each_cell or four_cell", ) diff --git a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py index 4f09c484aa0f4..01029004e6e47 100644 --- a/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py +++ b/vehicle/external_cmd_converter/launch/external_cmd_converter.launch.py @@ -25,7 +25,6 @@ def _create_mapping_tuple(name): def generate_launch_description(): - arguments = [ # component DeclareLaunchArgument("use_intra_process"),