diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 03f9f22b56..82f7197d87 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -21,11 +21,11 @@ common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -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 +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@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 common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/perception_utils/** mingyu.li@tier4.jp satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp +common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp @@ -34,7 +34,7 @@ common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato. common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp -common/tier4_autoware_utils/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @@ -46,6 +46,8 @@ common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@ti common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp +common/tier4_system_rviz_plugin/** koji.minoda@tier4.jp +common/tier4_target_object_type_rviz_plugin/** takamasa.horibe@tier4.jp common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -85,7 +87,7 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ar_tag_based_localizer/** masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -localization/localization_error_monitor/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -102,8 +104,9 @@ map/map_projection_loader/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yam map/map_tf_generator/** azumi.suzuki@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp +perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp -perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp +perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp @@ -118,7 +121,7 @@ perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@ perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp @@ -137,10 +140,10 @@ perception/tensorrt_yolo/** daisuke.nishimatsu@tier4.jp perception/tensorrt_yolox/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp perception/traffic_light_arbiter/** kenzo.lobos@tier4.jp shunsuke.miura@tier4.jp perception/traffic_light_classifier/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp -perception/traffic_light_fine_detector/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_fine_detector/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_map_based_detector/** yukihiro.saito@tier4.jp -perception/traffic_light_multi_camera_fusion/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp -perception/traffic_light_occlusion_predictor/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp +perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp +perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp @@ -173,8 +176,8 @@ planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp -planning/planning_validator/** takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_auto_mode_manager/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp @@ -185,7 +188,7 @@ planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/static_centerline_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp -sensing/gnss_poser/** yamato.ando@tier4.jp +sensing/gnss_poser/** koji.minoda@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp sensing/image_transport_decompressor/** yukihiro.saito@tier4.jp sensing/imu_corrector/** koji.minoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp diff --git a/common/autoware_auto_geometry/CMakeLists.txt b/common/autoware_auto_geometry/CMakeLists.txt index 454e0e7ef0..bf48e8fcac 100644 --- a/common/autoware_auto_geometry/CMakeLists.txt +++ b/common/autoware_auto_geometry/CMakeLists.txt @@ -12,6 +12,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/bounding_box.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(${PROJECT_NAME} PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + if(BUILD_TESTING) set(GEOMETRY_GTEST geometry_gtest) set(GEOMETRY_SRC test/src/test_geometry.cpp diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index a8ae9ec2d9..fe4cec1c0f 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -17,6 +17,11 @@ if(BUILD_TESTING) "tf2_geometry_msgs" "tf2_ros" ) + if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(test_tf2_autoware_auto_msgs PRIVATE + DEFINE_LEGACY_FUNCTION + ) + endif() endif() ament_auto_package() diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp index 9b20953a7b..ee8d52fa40 100644 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp @@ -45,7 +45,7 @@ using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; namespace tf2 { - +#ifdef DEFINE_LEGACY_FUNCTION /*************/ /** Point32 **/ /*************/ @@ -94,6 +94,7 @@ inline void doTransform( t_out.points[i].z = static_cast(v_out[2]); } } +#endif /******************/ /** Quaternion32 **/ diff --git a/common/geography_utils/src/lanelet2_projector.cpp b/common/geography_utils/src/lanelet2_projector.cpp index d487a5273c..3e982ac2cc 100644 --- a/common/geography_utils/src/lanelet2_projector.cpp +++ b/common/geography_utils/src/lanelet2_projector.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include @@ -30,14 +31,23 @@ std::unique_ptr get_lanelet2_projector(const MapProjectorInf lanelet::Origin origin{position}; lanelet::projection::UtmProjector projector{origin}; return std::make_unique(projector); + } else if (projector_info.projector_type == MapProjectorInfo::MGRS) { lanelet::projection::MGRSProjector projector{}; return std::make_unique(projector); - } else { - const std::string error_msg = "Invalid map projector type: " + projector_info.projector_type + - ". Currently supported types: MGRS, and LocalCartesianUTM"; - throw std::invalid_argument(error_msg); + + } else if (projector_info.projector_type == MapProjectorInfo::TRANSVERSE_MERCATOR) { + lanelet::GPSPoint position{ + projector_info.map_origin.latitude, projector_info.map_origin.longitude, + projector_info.map_origin.altitude}; + lanelet::Origin origin{position}; + lanelet::projection::TransverseMercatorProjector projector{origin}; + return std::make_unique(projector); } + const std::string error_msg = + "Invalid map projector type: " + projector_info.projector_type + + ". Currently supported types: MGRS, LocalCartesianUTM, and TransverseMercator"; + throw std::invalid_argument(error_msg); } } // namespace geography_utils diff --git a/common/motion_utils/CMakeLists.txt b/common/motion_utils/CMakeLists.txt index d1abf9dd73..43c8158b2f 100644 --- a/common/motion_utils/CMakeLists.txt +++ b/common/motion_utils/CMakeLists.txt @@ -7,13 +7,14 @@ autoware_package() find_package(Boost REQUIRED) ament_auto_add_library(motion_utils SHARED - src/motion_utils.cpp src/distance/distance.cpp src/marker/marker_helper.cpp src/marker/virtual_wall_marker_creator.cpp src/resample/resample.cpp + src/trajectory/trajectory.cpp src/trajectory/interpolation.cpp src/trajectory/path_with_lane_id.cpp + src/trajectory/tmp_conversion.cpp src/vehicle/vehicle_state_checker.cpp ) diff --git a/common/motion_utils/README.md b/common/motion_utils/README.md index 430014a300..925ee5b162 100644 --- a/common/motion_utils/README.md +++ b/common/motion_utils/README.md @@ -109,3 +109,9 @@ const size_t ego_nearest_seg_idx = findFirstNearestSegmentIndex(points, ego_pose const size_t dyn_obj_nearest_seg_idx = findFirstNearestSegmentIndex(points, dyn_obj_pose, dyn_obj_nearest_dist_threshold); const double length_from_ego_to_obj = calcSignedArcLength(points, ego_pose, ego_nearest_seg_idx, dyn_obj_pose, dyn_obj_nearest_seg_idx); ``` + +## For developers + +Some of the template functions in `trajectory.hpp` are mostly used for specific types (`autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::PathPoint`, `autoware_auto_planning_msgs::msg::TrajectoryPoint`), so they are exported as `extern template` functions to speed-up compilation time. + +`motion_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp index b6d10bbf40..0604eee653 100644 --- a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp +++ b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp @@ -15,13 +15,11 @@ #ifndef MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ #define MOTION_UTILS__MARKER__MARKER_HELPER_HPP_ -#include "motion_utils/resample/resample_utils.hpp" +#include #include -#include #include -#include namespace motion_utils { @@ -29,15 +27,18 @@ using geometry_msgs::msg::Pose; visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id, - const double longitudinal_offset = 0.0, const std::string & ns_prefix = ""); + const double longitudinal_offset = 0.0, const std::string & ns_prefix = "", + const bool is_driving_forward = true); visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker( const rclcpp::Time & now, const int32_t id); diff --git a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp index afead3a361..95bbcd93e3 100644 --- a/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp +++ b/common/motion_utils/include/motion_utils/marker/virtual_wall_marker_creator.hpp @@ -15,14 +15,14 @@ #ifndef MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ #define MOTION_UTILS__MARKER__VIRTUAL_WALL_MARKER_CREATOR_HPP_ -#include "motion_utils/marker/marker_helper.hpp" +#include #include +#include #include #include #include - namespace motion_utils { @@ -36,6 +36,7 @@ struct VirtualWall std::string ns{}; VirtualWallType style = stop; double longitudinal_offset{}; + bool is_driving_forward{true}; }; typedef std::vector VirtualWalls; @@ -52,7 +53,7 @@ class VirtualWallMarkerCreator using create_wall_function = std::function; + const std::string & ns_prefix, const bool is_driving_forward)>; VirtualWalls virtual_walls; std::unordered_map marker_count_per_namespace; diff --git a/common/motion_utils/include/motion_utils/motion_utils.hpp b/common/motion_utils/include/motion_utils/motion_utils.hpp deleted file mode 100644 index f6c94fbdac..0000000000 --- a/common/motion_utils/include/motion_utils/motion_utils.hpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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 MOTION_UTILS__MOTION_UTILS_HPP_ -#define MOTION_UTILS__MOTION_UTILS_HPP_ - -#include "motion_utils/constants.hpp" -#include "motion_utils/marker/marker_helper.hpp" -#include "motion_utils/resample/resample.hpp" -#include "motion_utils/trajectory/interpolation.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" -#include "motion_utils/trajectory/tmp_conversion.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "motion_utils/vehicle/vehicle_state_checker.hpp" - -#endif // MOTION_UTILS__MOTION_UTILS_HPP_ diff --git a/common/motion_utils/include/motion_utils/resample/resample.hpp b/common/motion_utils/include/motion_utils/resample/resample.hpp index cfb945270e..106ed32b18 100644 --- a/common/motion_utils/include/motion_utils/resample/resample.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample.hpp @@ -15,17 +15,6 @@ #ifndef MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ #define MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_ -#include "interpolation/interpolation_utils.hpp" -#include "interpolation/linear_interpolation.hpp" -#include "interpolation/spline_interpolation.hpp" -#include "interpolation/zero_order_hold.hpp" -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" - -#include - #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 2221bd0147..1e4d0d6333 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index bd6b9f10fa..74862a6229 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -15,90 +15,24 @@ #ifndef MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ #define MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include #include -#include #include -#include - namespace motion_utils { -inline boost::optional> getPathIndexRangeWithLaneId( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) -{ - size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error - size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error - - bool found_first_idx = false; - for (size_t i = 0; i < path.points.size(); ++i) { - const auto & p = path.points.at(i); - for (const auto & id : p.lane_ids) { - if (id == target_lane_id) { - if (!found_first_idx) { - start_idx = i; - found_first_idx = true; - } - end_idx = i; - } - } - } - - if (found_first_idx) { - // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. - start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); - end_idx = std::min(path.points.size() - 1, end_idx + 1); +boost::optional> getPathIndexRangeWithLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); - return std::make_pair(start_idx, end_idx); - } - - return {}; -} - -inline size_t findNearestIndexFromLaneId( +size_t findNearestIndexFromLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); - if (opt_range) { - const size_t start_idx = opt_range->first; - const size_t end_idx = opt_range->second; - - validateNonEmpty(path.points); + const geometry_msgs::msg::Point & pos, const int64_t lane_id); - const auto sub_points = std::vector{ - path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; - validateNonEmpty(sub_points); - - return start_idx + findNearestIndex(sub_points, pos); - } - - return findNearestIndex(path.points, pos); -} - -inline size_t findNearestSegmentIndexFromLaneId( +size_t findNearestSegmentIndexFromLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Point & pos, const int64_t lane_id) -{ - const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); - - if (nearest_idx == 0) { - return 0; - } - if (nearest_idx == path.points.size() - 1) { - return path.points.size() - 2; - } - - const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); - - if (signed_length <= 0) { - return nearest_idx - 1; - } - - return nearest_idx; -} + const geometry_msgs::msg::Point & pos, const int64_t lane_id); // @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle // center follow the input path diff --git a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp index 3bf06fa241..6921c56e01 100644 --- a/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/tmp_conversion.hpp @@ -15,15 +15,9 @@ #ifndef MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ #define MOTION_UTILS__TRAJECTORY__TMP_CONVERSION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include - -#include #include namespace motion_utils @@ -39,30 +33,15 @@ namespace motion_utils * @todo Decide how to handle the situation that we need to use the trajectory with the size of * points larger than the capacity. (Tier IV) */ -inline autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( - const std::vector & trajectory) -{ - autoware_auto_planning_msgs::msg::Trajectory output{}; - for (const auto & pt : trajectory) { - output.points.push_back(pt); - if (output.points.size() >= output.CAPACITY) { - break; - } - } - return output; -} +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory); /** * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to * std::vector. */ -inline std::vector convertToTrajectoryPointArray( - const autoware_auto_planning_msgs::msg::Trajectory & trajectory) -{ - std::vector output(trajectory.points.size()); - std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); - return output; -} +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory); } // namespace motion_utils diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index c65a7eae18..94cf48f29c 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -15,11 +15,16 @@ #ifndef MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ #define MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ -#include "interpolation/spline_interpolation_points_2d.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/constants.hpp" +#include + +#include +#include +#include + #include #include @@ -27,7 +32,6 @@ #include #include #include - namespace motion_utils { @@ -43,6 +47,15 @@ void validateNonEmpty(const T & points) } } +extern template void validateNonEmpty>( + const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); +extern template void +validateNonEmpty>( + const std::vector &); + /** * @brief validate a point is in a non-sharp angle between two points or not * @param point1 front point @@ -77,7 +90,7 @@ void validateNonSharpAngle( * @return (forward / backward) driving (true / false) */ template -boost::optional isDrivingForward(const T points) +boost::optional isDrivingForward(const T & points) { if (points.size() < 2) { return boost::none; @@ -90,6 +103,16 @@ boost::optional isDrivingForward(const T points) return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); } +extern template boost::optional +isDrivingForward>( + const std::vector &); +extern template boost::optional +isDrivingForward>( + const std::vector &); +extern template boost::optional +isDrivingForward>( + const std::vector &); + /** * @brief checks whether a path of trajectory has forward driving direction using its longitudinal * velocity @@ -97,7 +120,7 @@ boost::optional isDrivingForward(const T points) * @return (forward / backward) driving (true, false, none "if velocity is zero") */ template -boost::optional isDrivingForwardWithTwist(const T points_with_twist) +boost::optional isDrivingForwardWithTwist(const T & points_with_twist) { if (points_with_twist.empty()) { return boost::none; @@ -115,6 +138,16 @@ boost::optional isDrivingForwardWithTwist(const T points_with_twist) return isDrivingForward(points_with_twist); } +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +extern template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); + /** * @brief remove overlapping points through points container. * Overlapping is determined by calculating the distance between 2 consecutive points. @@ -125,7 +158,7 @@ boost::optional isDrivingForwardWithTwist(const T points_with_twist) * @return points container without overlapping points */ template -T removeOverlapPoints(const T & points, const size_t & start_idx = 0) +T removeOverlapPoints(const T & points, const size_t start_idx = 0) { if (points.size() < start_idx + 1) { return points; @@ -151,6 +184,19 @@ T removeOverlapPoints(const T & points, const size_t & start_idx = 0) return dst; } +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); +extern template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx = 0); + /** * @brief search through points container from specified start and end indices about first matching * index of a zero longitudinal velocity point. @@ -180,6 +226,11 @@ boost::optional searchZeroVelocityIndex( return {}; } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + /** * @brief search through points container from specified start index till end of points container * about first matching index of a zero longitudinal velocity point. @@ -188,7 +239,7 @@ boost::optional searchZeroVelocityIndex( * @return first matching index of a zero velocity point inside the points container. */ template -boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t & src_idx) +boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) { try { validateNonEmpty(points_with_twist); @@ -200,6 +251,11 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist, con return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + /** * @brief search through points container from its start to end about first matching index of a zero * longitudinal velocity point. @@ -212,6 +268,10 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist) return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); } +extern template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + /** * @brief find nearest point index through points container for a given point. * Finding nearest point is determined by looping through the points container, @@ -240,6 +300,18 @@ size_t findNearestIndex(const T & points, const geometry_msgs::msg::Point & poin return min_idx; } +extern template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + /** * @brief find nearest point index through points container for a given pose. * Finding nearest point is determined by looping through the points container, @@ -291,6 +363,22 @@ boost::optional findNearestIndex( return is_nearest_found ? boost::optional(min_idx) : boost::none; } +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + /** * @brief calculate longitudinal offset (length along trajectory from seg_idx point to nearest point * to p_target on trajectory). If seg_idx point is after that nearest point, length is negative. @@ -346,6 +434,21 @@ double calcLongitudinalOffsetToSegment( return segment_vec.dot(target_vec) / segment_vec.norm(); } +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); +extern template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + const bool throw_exception = false); + /** * @brief find nearest segment index to point. * Segment is straight path between two continuous points of trajectory. @@ -375,6 +478,19 @@ size_t findNearestSegmentIndex(const T & points, const geometry_msgs::msg::Point return nearest_idx; } +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +extern template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + /** * @brief find nearest segment index to pose * Segment is straight path between two continuous points of trajectory. @@ -413,6 +529,22 @@ boost::optional findNearestSegmentIndex( return *nearest_idx; } +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + /** * @brief calculate lateral offset from p_target (length from p_target to trajectory) using given * segment index. Segment is straight path between two continuous points of trajectory. @@ -459,6 +591,21 @@ double calcLateralOffset( return cross_vec(2) / segment_vec.norm(); } +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, + const bool throw_exception = false); + /** * @brief calculate lateral offset from p_target (length from p_target to trajectory). * The function gets the nearest segment index between the points of trajectory and the given target @@ -499,6 +646,18 @@ double calcLateralOffset( return calcLateralOffset(points, p_target, seg_idx, throw_exception); } +extern template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); +extern template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception = false); + /** * @brief calculate length of 2D distance between two points, specified by start and end points * indicies through points container. @@ -530,6 +689,19 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t return dist_sum; } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + /** * @brief Computes the partial sums of the elements in the sub-ranges of the range [src_idx, * dst_idx) and return these sum as vector @@ -567,6 +739,19 @@ std::vector calcSignedArcLengthPartialSum( return partial_dist; } +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +extern template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point and end point * index of points container. @@ -597,6 +782,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start index of points * container and end point. @@ -621,6 +819,19 @@ double calcSignedArcLength( return -calcSignedArcLength(points, dst_point, src_idx); } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); + /** * @brief calculate length of 2D distance between two points, specified by start point and end * point. @@ -656,6 +867,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + /** * @brief calculate length of 2D distance for whole points container, from its start to its end. * @param points points of trajectory, path, ... @@ -674,6 +898,15 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } +extern template double calcArcLength>( + const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); +extern template double +calcArcLength>( + const std::vector & points); + /** * @brief calculate curvature through points container. * The method used for calculating the curvature is using 3 consecutive points through the points @@ -684,7 +917,7 @@ double calcArcLength(const T & points) * @return calculated curvature container through points container */ template -inline std::vector calcCurvature(const T & points) +std::vector calcCurvature(const T & points) { std::vector curvature_vec(points.size()); @@ -700,6 +933,16 @@ inline std::vector calcCurvature(const T & points) return curvature_vec; } +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); +extern template std::vector +calcCurvature>( + const std::vector & points); + /** * @brief calculate curvature through points container and length of 2d distance for segment used * for curvature calculation. The method used for calculating the curvature is using 3 consecutive @@ -711,7 +954,7 @@ inline std::vector calcCurvature(const T & points) * curvature calculation */ template -inline std::vector> calcCurvatureAndArcLength(const T & points) +std::vector> calcCurvatureAndArcLength(const T & points) { // Note that arclength is for the segment, not the sum. std::vector> curvature_arc_length_vec; @@ -730,6 +973,16 @@ inline std::vector> calcCurvatureAndArcLength(const T return curvature_arc_length_vec; } +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +extern template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + /** * @brief calculate length of 2D distance between given start point index in points container and * first point in container with zero longitudinal velocity @@ -757,6 +1010,11 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); } +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx = 0); + /** * @brief calculate the point offset from source point index along the trajectory (or path) (points * container) @@ -767,7 +1025,7 @@ boost::optional calcDistanceToForwardStopPoint( * @return offset point */ template -inline boost::optional calcLongitudinalOffsetPoint( +boost::optional calcLongitudinalOffsetPoint( const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { try { @@ -821,6 +1079,19 @@ inline boost::optional calcLongitudinalOffsetPoint( return {}; } +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception = false); + /** * @brief calculate the point offset from source point along the trajectory (or path) (points * container) @@ -830,7 +1101,7 @@ inline boost::optional calcLongitudinalOffsetPoint( * @return offset point */ template -inline boost::optional calcLongitudinalOffsetPoint( +boost::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { try { @@ -853,6 +1124,19 @@ inline boost::optional calcLongitudinalOffsetPoint( return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); } +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +extern template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + /** * @brief calculate the point offset from source point index along the trajectory (or path) (points * container) @@ -864,7 +1148,7 @@ inline boost::optional calcLongitudinalOffsetPoint( * @return offset pose */ template -inline boost::optional calcLongitudinalOffsetPose( +boost::optional calcLongitudinalOffsetPose( const T & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false) { @@ -935,6 +1219,22 @@ inline boost::optional calcLongitudinalOffsetPose( return {}; } +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction = true, + const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, + const bool set_orientation_from_position_direction = true, const bool throw_exception = false); + /** * @brief calculate the point offset from source point along the trajectory (or path) (points * container) @@ -946,7 +1246,7 @@ inline boost::optional calcLongitudinalOffsetPose( * @return offset pase */ template -inline boost::optional calcLongitudinalOffsetPose( +boost::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true) { @@ -966,6 +1266,22 @@ inline boost::optional calcLongitudinalOffsetPose( set_orientation_from_position_direction); } +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); +extern template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction = true); + /** * @brief insert a point in points container (trajectory, path, ...) using segment id * @param seg_idx segment index of point at beginning of length @@ -976,7 +1292,7 @@ inline boost::optional calcLongitudinalOffsetPose( * @return index of segment id, where point is inserted */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1052,6 +1368,22 @@ inline boost::optional insertTargetPoint( return seg_idx; } +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief insert a point in points container (trajectory, path, ...) using length of point to be * inserted @@ -1063,7 +1395,7 @@ inline boost::optional insertTargetPoint( * @return index of segment id, where point is inserted */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1076,6 +1408,7 @@ inline boost::optional insertTargetPoint( // Get Nearest segment index boost::optional segment_idx = boost::none; for (size_t i = 1; i < points.size(); ++i) { + // TODO(Mamoru Sobue): find accumulated sum beforehand const double length = calcSignedArcLength(points, 0, i); if (insert_point_length <= length) { segment_idx = i - 1; @@ -1090,6 +1423,22 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief insert a point in points container (trajectory, path, ...) using segment index and length * of point to be inserted @@ -1101,7 +1450,7 @@ inline boost::optional insertTargetPoint( * @return index of insert point */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const size_t src_segment_idx, const double insert_point_length, T & points, const double overlap_threshold = 1e-3) { @@ -1147,6 +1496,22 @@ inline boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold = 1e-3); + /** * @brief Insert a target point from a source pose on the trajectory * @param src_pose source pose on the trajectory @@ -1161,7 +1526,7 @@ inline boost::optional insertTargetPoint( * @return index of insert point */ template -inline boost::optional insertTargetPoint( +boost::optional insertTargetPoint( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1184,6 +1549,25 @@ inline boost::optional insertTargetPoint( *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); } +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + /** * @brief Insert stop point from the source segment index * @param src_segment_idx start segment index on the trajectory @@ -1194,7 +1578,7 @@ inline boost::optional insertTargetPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { @@ -1217,6 +1601,22 @@ inline boost::optional insertStopPoint( return stop_idx; } +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert stop point from the front point of the path * @param distance_to_stop_point distance to stop point from the front point of the path @@ -1226,7 +1626,7 @@ inline boost::optional insertStopPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { validateNonEmpty(points_with_twist); @@ -1250,6 +1650,22 @@ inline boost::optional insertStopPoint( return boost::none; } +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert Stop point from the source pose * @param src_pose source pose @@ -1264,7 +1680,7 @@ inline boost::optional insertStopPoint( * @return index of stop point */ template -inline boost::optional insertStopPoint( +boost::optional insertStopPoint( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, T & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1289,6 +1705,25 @@ inline boost::optional insertStopPoint( return stop_idx; } +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); +extern template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); + /** * @brief Insert Stop point that is in the stop segment index * @param stop_seg_idx segment index of the stop pose @@ -1317,6 +1752,12 @@ boost::optional insertStopPoint( return insert_idx; } +extern template boost::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + /** * @brief Insert deceleration point from the source pose * @param src_point source point @@ -1353,6 +1794,12 @@ boost::optional insertDecelPoint( return insert_idx; } +extern template boost::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + /** * @brief Insert orientation to each point in points container (trajectory, path, ...) * @param points points of trajectory, path, ... (input / output) @@ -1390,9 +1837,21 @@ void insertOrientation(T & points, const bool is_driving_forward) } } +extern template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +extern template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); +extern template void +insertOrientation>( + std::vector & points, + const bool is_driving_forward); + /** * @brief Remove points with invalid orientation differences from a given points container - * (trajectory, path, ...) + * (trajectory, path, ...). Check the difference between the angles of two points and the difference + * between the azimuth angle between the two points and the angle of the next point. * @param points Points of trajectory, path, or other point container (input / output) * @param max_yaw_diff Maximum acceptable yaw angle difference between two consecutive points in * radians (default: M_PI_2) @@ -1401,10 +1860,14 @@ template void removeInvalidOrientationPoints(T & points, const double max_yaw_diff = M_PI_2) { for (size_t i = 1; i < points.size();) { - const double yaw1 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i - 1)).orientation); - const double yaw2 = tf2::getYaw(tier4_autoware_utils::getPose(points.at(i)).orientation); - - if (max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2))) { + const auto p1 = tier4_autoware_utils::getPose(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPose(points.at(i)); + const double yaw1 = tf2::getYaw(p1.orientation); + const double yaw2 = tf2::getYaw(p2.orientation); + + if ( + max_yaw_diff < std::abs(tier4_autoware_utils::normalizeRadian(yaw1 - yaw2)) || + !tier4_autoware_utils::isDrivingForward(p1, p2)) { points.erase(points.begin() + i); } else { ++i; @@ -1440,6 +1903,22 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point and its * segment index in points container and end point index in points container @@ -1465,6 +1944,19 @@ double calcSignedArcLength( return signed_length_on_traj - signed_length_src_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + /** * @brief calculate length of 2D distance between two points, specified by start point index in * points container and end point and its segment index in points container @@ -1490,6 +1982,19 @@ double calcSignedArcLength( return signed_length_on_traj + signed_length_dst_offset; } +extern template double +calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +extern template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + /** * @brief find first nearest point index through points container for a given pose with soft * distance and yaw constraints. Finding nearest point is determined by looping through the points @@ -1580,6 +2085,25 @@ size_t findFirstNearestIndexWithSoftConstraints( return findNearestIndex(points, pose.position); } +extern template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + /** * @brief find nearest segment index to pose with soft constraints * Segment is straight path between two continuous points of trajectory @@ -1617,6 +2141,25 @@ size_t findFirstNearestSegmentIndexWithSoftConstraints( return nearest_idx; } +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); +extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, + const double dist_threshold = std::numeric_limits::max(), + const double yaw_threshold = std::numeric_limits::max()); + /** * @brief calculate the point offset from source point along the trajectory (or path) * @brief calculate length of 2D distance between given pose and first point in container with zero @@ -1663,6 +2206,22 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, closest_stop_dist); } +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); +extern template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), + const double max_yaw = std::numeric_limits::max()); + // NOTE: Points after forward length from the point will be cropped // forward_length is assumed to be positive. template @@ -1687,6 +2246,22 @@ T cropForwardPoints( return points; } +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +extern template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + // NOTE: Points before backward length from the point will be cropped // backward_length is assumed to be positive. template @@ -1711,6 +2286,22 @@ T cropBackwardPoints( return points; } +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +extern template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + template T cropPoints( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, @@ -1739,6 +2330,22 @@ T cropPoints( return cropped_points; } +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +extern template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + /** * @brief Calculate the angle of the input pose with respect to the nearest trajectory segment. * The function gets the nearest segment index between the points of the trajectory and the given @@ -1785,6 +2392,18 @@ double calcYawDeviation( return tier4_autoware_utils::normalizeRadian(pose_yaw - path_yaw); } +extern template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); +extern template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception = false); + /** * @brief Check if the given target point is in front of the based pose from the trajectory. * if the points is empty, the function returns false @@ -1808,6 +2427,22 @@ bool isTargetPointFront( return s_target - s_base > threshold; } + +extern template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); +extern template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold = 0.0); + } // namespace motion_utils #endif // MOTION_UTILS__TRAJECTORY__TRAJECTORY_HPP_ diff --git a/common/motion_utils/package.xml b/common/motion_utils/package.xml index 834342624b..334b023f7f 100644 --- a/common/motion_utils/package.xml +++ b/common/motion_utils/package.xml @@ -13,6 +13,7 @@ Taiki Tanaka Takamasa Horibe Tomoya Kimura + Mamoru Sobue Apache License 2.0 Yutaka Shimizu diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp index c869271222..50def253b6 100644 --- a/common/motion_utils/src/marker/marker_helper.cpp +++ b/common/motion_utils/src/marker/marker_helper.cpp @@ -14,11 +14,12 @@ #include "motion_utils/marker/marker_helper.hpp" +#include "motion_utils/resample/resample_utils.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" #include -#include +#include using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -90,10 +91,11 @@ namespace motion_utils { visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5)); @@ -101,10 +103,11 @@ visualization_msgs::msg::MarkerArray createStopVirtualWallMarker( visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5)); @@ -112,10 +115,11 @@ visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker( visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker( const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now, - const int32_t id, const double longitudinal_offset, const std::string & ns_prefix) + const int32_t id, const double longitudinal_offset, const std::string & ns_prefix, + const bool is_driving_forward) { - const auto pose_with_offset = - tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0); + const auto pose_with_offset = tier4_autoware_utils::calcOffsetPose( + pose, longitudinal_offset * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); return createVirtualWallMarkerArray( pose_with_offset, module_name, ns_prefix + "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5)); diff --git a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp index 114c4f8790..c22aaa0d5e 100644 --- a/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp +++ b/common/motion_utils/src/marker/virtual_wall_marker_creator.cpp @@ -14,7 +14,7 @@ #include "motion_utils/marker/virtual_wall_marker_creator.hpp" -#include +#include "motion_utils/marker/marker_helper.hpp" namespace motion_utils { @@ -66,7 +66,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::create_markers( } auto markers = create_fn( virtual_wall.pose, virtual_wall.text, now, 0, virtual_wall.longitudinal_offset, - virtual_wall.ns); + virtual_wall.ns, virtual_wall.is_driving_forward); for (auto & marker : markers.markers) { marker.id = marker_count_per_namespace[marker.ns].current++; marker_array.markers.push_back(marker); diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp index 1a800a38d1..6bada92180 100644 --- a/common/motion_utils/src/resample/resample.cpp +++ b/common/motion_utils/src/resample/resample.cpp @@ -14,8 +14,15 @@ #include "motion_utils/resample/resample.hpp" +#include "interpolation/interpolation_utils.hpp" +#include "interpolation/linear_interpolation.hpp" +#include "interpolation/spline_interpolation.hpp" +#include "interpolation/zero_order_hold.hpp" #include "motion_utils/resample/resample_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" +#include "tier4_autoware_utils/math/constants.hpp" namespace motion_utils { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index 759f0ef4ed..c74db0f61d 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -14,8 +14,90 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "interpolation/spline_interpolation_points_2d.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + +#include +#include +#include + namespace motion_utils { + +boost::optional> getPathIndexRangeWithLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) +{ + size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error + size_t end_idx = 0; // NOTE: to prevent from maybe-uninitialized error + + bool found_first_idx = false; + for (size_t i = 0; i < path.points.size(); ++i) { + const auto & p = path.points.at(i); + for (const auto & id : p.lane_ids) { + if (id == target_lane_id) { + if (!found_first_idx) { + start_idx = i; + found_first_idx = true; + } + end_idx = i; + } + } + } + + if (found_first_idx) { + // NOTE: In order to fully cover lanes with target_lane_id, start_idx and end_idx are expanded. + start_idx = static_cast(std::max(0, static_cast(start_idx) - 1)); + end_idx = std::min(path.points.size() - 1, end_idx + 1); + + return std::make_pair(start_idx, end_idx); + } + + return {}; +} + +size_t findNearestIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const auto opt_range = getPathIndexRangeWithLaneId(path, lane_id); + if (opt_range) { + const size_t start_idx = opt_range->first; + const size_t end_idx = opt_range->second; + + validateNonEmpty(path.points); + + const auto sub_points = std::vector{ + path.points.begin() + start_idx, path.points.begin() + end_idx + 1}; + validateNonEmpty(sub_points); + + return start_idx + findNearestIndex(sub_points, pos); + } + + return findNearestIndex(path.points, pos); +} + +size_t findNearestSegmentIndexFromLaneId( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, + const geometry_msgs::msg::Point & pos, const int64_t lane_id) +{ + const size_t nearest_idx = findNearestIndexFromLaneId(path, pos, lane_id); + + if (nearest_idx == 0) { + return 0; + } + if (nearest_idx == path.points.size() - 1) { + return path.points.size() - 2; + } + + const double signed_length = calcLongitudinalOffsetToSegment(path.points, nearest_idx, pos); + + if (signed_length <= 0) { + return nearest_idx - 1; + } + + return nearest_idx; +} + // NOTE: rear_to_cog is supposed to be positive autoware_auto_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog, diff --git a/common/motion_utils/src/trajectory/tmp_conversion.cpp b/common/motion_utils/src/trajectory/tmp_conversion.cpp new file mode 100644 index 0000000000..5c907a5926 --- /dev/null +++ b/common/motion_utils/src/trajectory/tmp_conversion.cpp @@ -0,0 +1,60 @@ +// 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 "motion_utils/trajectory/tmp_conversion.hpp" + +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include + +namespace motion_utils +{ +/** + * @brief Convert std::vector to + * autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to + * autoware_auto_msgs. We should consider whether to remove this function after the porting is done. + * @attention This function just clips + * std::vector up to the capacity of Trajectory. + * Therefore, the error handling out of this function is necessary if the size of the input greater + * than the capacity. + * @todo Decide how to handle the situation that we need to use the trajectory with the size of + * points larger than the capacity. (Tier IV) + */ +autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory( + const std::vector & trajectory) +{ + autoware_auto_planning_msgs::msg::Trajectory output{}; + for (const auto & pt : trajectory) { + output.points.push_back(pt); + if (output.points.size() >= output.CAPACITY) { + break; + } + } + return output; +} + +/** + * @brief Convert autoware_auto_planning_msgs::msg::Trajectory to + * std::vector. + */ +std::vector convertToTrajectoryPointArray( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory) +{ + std::vector output(trajectory.points.size()); + std::copy(trajectory.points.begin(), trajectory.points.end(), output.begin()); + return output; +} + +} // namespace motion_utils diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp new file mode 100644 index 0000000000..12074e537c --- /dev/null +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -0,0 +1,625 @@ +// 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 "motion_utils/trajectory/trajectory.hpp" + +namespace motion_utils +{ + +// +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); +template void validateNonEmpty>( + const std::vector &); + +// +template boost::optional +isDrivingForward>( + const std::vector &); +template boost::optional +isDrivingForward>( + const std::vector &); +template boost::optional +isDrivingForward>( + const std::vector &); + +// +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); +template boost::optional +isDrivingForwardWithTwist>( + const std::vector &); + +// +template std::vector +removeOverlapPoints>( + const std::vector & points, const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); +template std::vector +removeOverlapPoints>( + const std::vector & points, + const size_t start_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx, const size_t dst_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template boost::optional +searchZeroVelocityIndex>( + const std::vector & points_with_twist); + +// +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, const size_t seg_idx, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLongitudinalOffsetToSegment>( + const std::vector & points, + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template size_t findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); +template size_t +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Point & point); + +// +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); +template boost::optional +findNearestSegmentIndex>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const size_t seg_idx, const bool throw_exception); + +// +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double +calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); +template double calcLateralOffset>( + const std::vector & points, + const geometry_msgs::msg::Point & p_target, const bool throw_exception); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + +// +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, const size_t src_idx, + const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); +template std::vector +calcSignedArcLengthPartialSum>( + const std::vector & points, + const size_t src_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector &, + const geometry_msgs::msg::Point & src_point, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const geometry_msgs::msg::Point & dst_point); + +// +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); +template double calcArcLength>( + const std::vector & points); + +// +template std::vector +calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); +template std::vector +calcCurvature>( + const std::vector & points); + +// +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); +template std::vector> +calcCurvatureAndArcLength>( + const std::vector & points); + +// +template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const size_t src_idx); + +// +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, const size_t src_idx, + const double offset, const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const size_t src_idx, const double offset, const bool throw_exception); + +// +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); +template boost::optional +calcLongitudinalOffsetPoint>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset); + +// +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, const size_t src_idx, + const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, + const bool throw_exception); + +// +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); +template boost::optional +calcLongitudinalOffsetPose>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const double offset, + const bool set_orientation_from_position_direction); + +// +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t seg_idx, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const double insert_point_length, const geometry_msgs::msg::Point & p_target, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const size_t src_segment_idx, const double insert_point_length, + std::vector & points, + const double overlap_threshold); + +// +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertTargetPoint>( + const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, + std::vector & points, const double max_dist, + const double max_yaw, const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); +template boost::optional +insertStopPoint>( + const size_t src_segment_idx, const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold = 1e-3); + +// +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); +template boost::optional +insertStopPoint>( + const double distance_to_stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); +template boost::optional +insertStopPoint>( + const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, + std::vector & points_with_twist, + const double max_dist, const double max_yaw, const double overlap_threshold); + +// +template boost::optional +insertStopPoint>( + const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, + std::vector & points_with_twist, + const double overlap_threshold); + +// +template boost::optional +insertDecelPoint>( + const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, + const double velocity, + std::vector & points_with_twist); + +// +template void insertOrientation>( + std::vector & points, const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); +template void insertOrientation>( + std::vector & points, + const bool is_driving_forward); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); +template double calcSignedArcLength>( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, const size_t src_seg_idx, const size_t dst_idx); + +// +template double calcSignedArcLength>( + const std::vector & points, const size_t src_idx, + const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double +calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); +template double calcSignedArcLength>( + const std::vector & points, + const size_t src_idx, const geometry_msgs::msg::Point & dst_point, const size_t dst_seg_idx); + +// +template size_t +findFirstNearestIndexWithSoftConstraints>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); +template size_t findFirstNearestSegmentIndexWithSoftConstraints< + std::vector>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); + +// +template boost::optional +calcDistanceToForwardStopPoint>( + const std::vector & points_with_twist, + const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); + +// +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); +template std::vector +cropForwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length); + +// +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); +template std::vector +cropBackwardPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double backward_length); + +// +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); +template std::vector +cropPoints>( + const std::vector & points, + const geometry_msgs::msg::Point & target_pos, const size_t target_seg_idx, + const double forward_length, const double backward_length); + +// +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double +calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); +template double calcYawDeviation>( + const std::vector & points, + const geometry_msgs::msg::Pose & pose, const bool throw_exception); + +// +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool +isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); +template bool isTargetPointFront>( + const std::vector & points, + const geometry_msgs::msg::Point & base_point, const geometry_msgs::msg::Point & target_point, + const double threshold); + +} // namespace motion_utils diff --git a/common/motion_utils/test/src/resample/test_resample.cpp b/common/motion_utils/test/src/resample/test_resample.cpp index d7ce15b4c3..373a12c5bb 100644 --- a/common/motion_utils/test/src/resample/test_resample.cpp +++ b/common/motion_utils/test/src/resample/test_resample.cpp @@ -15,6 +15,7 @@ #include "motion_utils/constants.hpp" #include "motion_utils/resample/resample.hpp" #include "tier4_autoware_utils/geometry/boost_geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/motion_utils/test/src/test_motion_utils.cpp b/common/motion_utils/test/src/test_motion_utils.cpp index a8d9f76116..d8b9b0ac78 100644 --- a/common/motion_utils/test/src/test_motion_utils.cpp +++ b/common/motion_utils/test/src/test_motion_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/motion_utils.hpp" +#include #include diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 121a9fa69b..4df1ef0a09 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -13,8 +13,7 @@ // limitations under the License. #include "motion_utils/trajectory/path_with_lane_id.hpp" -// #include "tier4_autoware_utils/geometry/geometry.hpp" -// #include "tier4_autoware_utils/math/unit_conversion.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include 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 ff448e694e..058a5d5deb 100644 --- a/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp +++ b/common/rtc_manager_rviz_plugin/src/rtc_manager_panel.cpp @@ -366,7 +366,10 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr 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; + if (msg->statuses.empty()) { + rtc_table_->update(); + return; + } // this is to stable rtc display not to occupy too much size_t min_display_size{5}; size_t max_display_size{10}; @@ -374,8 +377,17 @@ void RTCManagerPanel::onRTCStatus(const CooperateStatusArray::ConstSharedPtr msg rtc_table_->setRowCount( std::max(min_display_size, std::min(msg->statuses.size(), max_display_size))); int cnt = 0; - for (auto status : msg->statuses) { - if (static_cast(cnt) >= max_display_size) return; + + auto sorted_statuses = msg->statuses; + std::partition(sorted_statuses.begin(), sorted_statuses.end(), [](const auto & status) { + return !status.auto_mode && !uint2bool(status.command_status.type); + }); + + for (auto status : sorted_statuses) { + if (static_cast(cnt) >= max_display_size) { + rtc_table_->update(); + return; + } // uuid { std::stringstream uuid; diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index 071b55177a..938a78692d 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -7,11 +7,13 @@ autoware_package() find_package(Boost REQUIRED) ament_auto_add_library(tier4_autoware_utils SHARED - src/tier4_autoware_utils.cpp + src/geometry/geometry.cpp + src/geometry/pose_deviation.cpp src/geometry/boost_polygon_utils.cpp src/math/sin_table.cpp src/math/trigonometry.cpp src/ros/msg_operation.cpp + src/ros/marker_helper.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/README.md b/common/tier4_autoware_utils/README.md index 7a352e971a..ffcc414cd7 100644 --- a/common/tier4_autoware_utils/README.md +++ b/common/tier4_autoware_utils/README.md @@ -3,3 +3,7 @@ ## Purpose This package contains many common functions used by other packages, so please refer to them as needed. + +## For developers + +`tier4_autoware_utils.hpp` header file was removed because the source files that directly/indirectly include this file took a long time for preprocessing. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 91f9ab09a1..04bcfbacbb 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -21,7 +21,6 @@ #define EIGEN_MPL2_ONLY #include -#include #include diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 46cbd1f141..5d9001affa 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -22,9 +22,6 @@ #include #include -#include -#include - #include namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 8c3bcd966c..17ce7e1f4d 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -15,20 +15,20 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ -#include -#include -#include - -#define EIGEN_MPL2_ONLY #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include "tier4_autoware_utils/math/constants.hpp" #include "tier4_autoware_utils/math/normalization.hpp" #include "tier4_autoware_utils/ros/msg_covariance.hpp" +#include +#include +#include + +#define EIGEN_MPL2_ONLY #include -#include #include +#include #include #include #include @@ -38,27 +38,15 @@ #include #include #include +#include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - // TODO(wep21): Remove these apis // after they are implemented in ros2 geometry2. namespace tf2 { -inline void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) -{ - out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); - out.frame_id_ = msg.header.frame_id; - tf2::Transform tmp; - fromMsg(msg.pose, tmp); - out.setData(tmp); -} +void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out); #ifdef ROS_DISTRO_GALACTIC // Remove after this commit is released // https://github.com/ros2/geometry2/commit/e9da371d81e388a589540357c050e262442f1b4a @@ -146,6 +134,13 @@ inline geometry_msgs::msg::Point getPoint(const autoware_auto_planning_msgs::msg return p.pose.position; } +template <> +inline geometry_msgs::msg::Point getPoint( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose.position; +} + template <> inline geometry_msgs::msg::Point getPoint( const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) @@ -178,6 +173,13 @@ inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg:: return p.pose; } +template <> +inline geometry_msgs::msg::Pose getPose( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.pose; +} + template <> inline geometry_msgs::msg::Pose getPose(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { @@ -197,6 +199,13 @@ inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::Pa return p.longitudinal_velocity_mps; } +template <> +inline double getLongitudinalVelocity( + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + return p.point.longitudinal_velocity_mps; +} + template <> inline double getLongitudinalVelocity(const autoware_auto_planning_msgs::msg::TrajectoryPoint & p) { @@ -229,6 +238,13 @@ inline void setPose( p.pose = pose; } +template <> +inline void setPose( + const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + p.point.pose = pose; +} + template <> inline void setPose( const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::TrajectoryPoint & p) @@ -265,6 +281,13 @@ inline void setLongitudinalVelocity( p.longitudinal_velocity_mps = velocity; } +template <> +inline void setLongitudinalVelocity( + const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) +{ + p.point.longitudinal_velocity_mps = velocity; +} + inline geometry_msgs::msg::Point createPoint(const double x, const double y, const double z) { geometry_msgs::msg::Point p; @@ -274,66 +297,25 @@ inline geometry_msgs::msg::Point createPoint(const double x, const double y, con return p; } -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat) -{ - geometry_msgs::msg::Vector3 rpy; - tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); - tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); - return rpy; -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat); -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) -{ - return getRPY(pose.orientation); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose); -inline geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) -{ - return getRPY(pose.pose); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose); -inline geometry_msgs::msg::Vector3 getRPY( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose) -{ - return getRPY(pose.pose.pose); -} +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); -inline geometry_msgs::msg::Quaternion createQuaternion( - const double x, const double y, const double z, const double w) -{ - geometry_msgs::msg::Quaternion q; - q.x = x; - q.y = y; - q.z = z; - q.w = w; - return q; -} +geometry_msgs::msg::Quaternion createQuaternion( + const double x, const double y, const double z, const double w); -inline geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z) -{ - geometry_msgs::msg::Vector3 v; - v.x = x; - v.y = y; - v.z = z; - return v; -} +geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z); // Revival of tf::createQuaternionFromRPY // https://answers.ros.org/question/304397/recommended-way-to-construct-quaternion-from-rollpitchyaw-with-tf2/ -inline geometry_msgs::msg::Quaternion createQuaternionFromRPY( - const double roll, const double pitch, const double yaw) -{ - tf2::Quaternion q; - q.setRPY(roll, pitch, yaw); - return tf2::toMsg(q); -} +geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw); -inline geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw) -{ - tf2::Quaternion q; - q.setRPY(0, 0, yaw); - return tf2::toMsg(q); -} +geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw); template double calcDistance2d(const Point1 & point1, const Point2 & point2) @@ -371,13 +353,8 @@ double calcDistance3d(const Point1 & point1, const Point2 & point2) * @param p_to target point * @return -pi/2 <= elevation angle <= pi/2 */ -inline double calcElevationAngle( - const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) -{ - const double dz = p_to.z - p_from.z; - const double dist_2d = calcDistance2d(p_from, p_to); - return std::atan2(dz, dist_2d); -} +double calcElevationAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to); /** * @brief calculate azimuth angle of two points. @@ -388,52 +365,18 @@ inline double calcElevationAngle( * @param p_to target point * @return -pi < azimuth angle < pi. */ -inline double calcAzimuthAngle( - const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) -{ - const double dx = p_to.x - p_from.x; - const double dy = p_to.y - p_from.y; - return std::atan2(dy, dx); -} +double calcAzimuthAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to); -inline geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::Pose pose; - pose.position.x = transform.translation.x; - pose.position.y = transform.translation.y; - pose.position.z = transform.translation.z; - pose.orientation = transform.rotation; - return pose; -} +geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform); -inline geometry_msgs::msg::PoseStamped transform2pose( - const geometry_msgs::msg::TransformStamped & transform) -{ - geometry_msgs::msg::PoseStamped pose; - pose.header = transform.header; - pose.pose = transform2pose(transform.transform); - return pose; -} +geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform); -inline geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = pose.position.x; - transform.translation.y = pose.position.y; - transform.translation.z = pose.position.z; - transform.rotation = pose.orientation; - return transform; -} +geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose); -inline geometry_msgs::msg::TransformStamped pose2transform( - const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id) -{ - geometry_msgs::msg::TransformStamped transform; - transform.header = pose.header; - transform.transform = pose2transform(pose.pose); - transform.child_frame_id = child_frame_id; - return transform; -} +geometry_msgs::msg::TransformStamped pose2transform( + const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id); template tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) @@ -447,65 +390,18 @@ tf2::Vector3 point2tfVector(const Point1 & src, const Point2 & dst) return tf2::Vector3(dx, dy, dz); } -inline Point3d transformPoint( - const Point3d & point, const geometry_msgs::msg::Transform & transform) -{ - const auto & translation = transform.translation; - const auto & rotation = transform.rotation; - - const Eigen::Translation3d T(translation.x, translation.y, translation.z); - const Eigen::Quaterniond R(rotation.w, rotation.x, rotation.y, rotation.z); +Point3d transformPoint(const Point3d & point, const geometry_msgs::msg::Transform & transform); - const Eigen::Vector3d transformed(T * R * point); - - return Point3d{transformed.x(), transformed.y(), transformed.z()}; -} +Point2d transformPoint(const Point2d & point, const geometry_msgs::msg::Transform & transform); -inline Point2d transformPoint( - const Point2d & point, const geometry_msgs::msg::Transform & transform) -{ - Point3d point_3d{point.x(), point.y(), 0}; - const auto transformed = transformPoint(point_3d, transform); - return Point2d{transformed.x(), transformed.y()}; -} +Eigen::Vector3d transformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose); -inline Eigen::Vector3d transformPoint( - const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = pose.position.x; - transform.translation.y = pose.position.y; - transform.translation.z = pose.position.z; - transform.rotation = pose.orientation; +geometry_msgs::msg::Point transformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose); - Point3d p = transformPoint(Point3d(point.x(), point.y(), point.z()), transform); - return Eigen::Vector3d(p.x(), p.y(), p.z()); -} - -inline geometry_msgs::msg::Point transformPoint( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Vector3d vec = Eigen::Vector3d(point.x, point.y, point.z); - auto transformed_vec = transformPoint(vec, pose); - - geometry_msgs::msg::Point transformed_point; - transformed_point.x = transformed_vec.x(); - transformed_point.y = transformed_vec.y(); - transformed_point.z = transformed_vec.z(); - return transformed_point; -} - -inline geometry_msgs::msg::Point32 transformPoint( - const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose) -{ - const auto point = - geometry_msgs::build().x(point32.x).y(point32.y).z(point32.z); - const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); - return geometry_msgs::build() - .x(transformed_point.x) - .y(transformed_point.y) - .z(transformed_point.z); -} +geometry_msgs::msg::Point32 transformPoint( + const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose); template T transformVector(const T & points, const geometry_msgs::msg::Transform & transform) @@ -517,110 +413,40 @@ T transformVector(const T & points, const geometry_msgs::msg::Transform & transf return transformed; } -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) -{ - geometry_msgs::msg::Pose transformed_pose; - tf2::doTransform(pose, transformed_pose, transform); +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); - return transformed_pose; -} - -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) -{ - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = transform; - - return transformPose(pose, transform_stamped); -} - -inline geometry_msgs::msg::Pose transformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform) -{ - tf2::Transform transform; - tf2::convert(pose_transform, transform); +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform); - geometry_msgs::msg::TransformStamped transform_msg; - transform_msg.transform = tf2::toMsg(transform); - - return transformPose(pose, transform_msg); -} +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform); // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) -{ - tf2::Transform tf; - tf2::fromMsg(transform, tf); - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = tf2::toMsg(tf.inverse()); - - return transformPose(pose, transform_stamped); -} +/* +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform); +*/ // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) -{ - tf2::Transform tf; - tf2::fromMsg(transform, tf); - geometry_msgs::msg::TransformStamped transform_stamped; - transform_stamped.transform = tf2::toMsg(tf.inverse()); - - return transformPose(pose, transform_stamped); -} +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform); // Transform pose in world coordinates to local coordinates -inline geometry_msgs::msg::Pose inverseTransformPose( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose) -{ - tf2::Transform transform; - tf2::convert(transform_pose, transform); - - return inverseTransformPose(pose, tf2::toMsg(transform)); -} +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose); // Transform point in world coordinates to local coordinates -inline Eigen::Vector3d inverseTransformPoint( - const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Quaterniond q( - pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); - const Eigen::Matrix3d R = q.normalized().toRotationMatrix(); - - const Eigen::Vector3d local_origin(pose.position.x, pose.position.y, pose.position.z); - Eigen::Vector3d local_point = R.transpose() * point - R.transpose() * local_origin; - - return local_point; -} +Eigen::Vector3d inverseTransformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose); // Transform point in world coordinates to local coordinates -inline geometry_msgs::msg::Point inverseTransformPoint( - const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) -{ - const Eigen::Vector3d local_vec = - inverseTransformPoint(Eigen::Vector3d(point.x, point.y, point.z), pose); - geometry_msgs::msg::Point local_point; - local_point.x = local_vec.x(); - local_point.y = local_vec.y(); - local_point.z = local_vec.z(); - return local_point; -} +geometry_msgs::msg::Point inverseTransformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose); -inline double calcCurvature( +double calcCurvature( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3) -{ - // Calculation details are described in the following page - // https://en.wikipedia.org/wiki/Menger_curvature - const double denominator = - calcDistance2d(p1, p2) * calcDistance2d(p2, p3) * calcDistance2d(p3, p1); - if (std::fabs(denominator) < 1e-10) { - throw std::runtime_error("points are too close for curvature calculation."); - } - return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; -} + const geometry_msgs::msg::Point & p3); template bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) @@ -635,20 +461,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input * pose. */ -inline geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) -{ - geometry_msgs::msg::Pose pose; - geometry_msgs::msg::Transform transform; - transform.translation = createTranslation(x, y, z); - transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); - tf2::Transform tf_pose; - tf2::Transform tf_offset; - tf2::fromMsg(transform, tf_offset); - tf2::fromMsg(p, tf_pose); - tf2::toMsg(tf_pose * tf_offset, pose); - return pose; -} +geometry_msgs::msg::Pose calcOffsetPose( + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z); /** * @brief Calculate a point by linear interpolation. @@ -759,39 +573,13 @@ inline double calcNorm(const geometry_msgs::msg::Vector3 & v) * @return If all element of covariance is 0, return false. */ // -inline bool isTwistCovarianceValid( - const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) -{ - for (const auto & c : twist_with_covariance.covariance) { - if (c != 0.0) { - return true; - } - } - return false; -} +bool isTwistCovarianceValid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance); // NOTE: much faster than boost::geometry::intersects() -inline std::optional intersect( +std::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - // calculate intersection point - const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); - if (det == 0.0) { - return std::nullopt; - } + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); - const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; - const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; - if (t < 0 || 1 < t || s < 0 || 1 < s) { - return std::nullopt; - } - - geometry_msgs::msg::Point intersect_point; - intersect_point.x = t * p1.x + (1.0 - t) * p2.x; - intersect_point.y = t * p1.y + (1.0 - t) * p2.y; - return intersect_point; -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp deleted file mode 100644 index 91ba0c4d5d..0000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// 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 TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ -#define TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ - -#include "tier4_autoware_utils/geometry/geometry.hpp" - -#include - -namespace tier4_autoware_utils -{ -template <> -inline geometry_msgs::msg::Point getPoint( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose.position; -} - -template <> -inline geometry_msgs::msg::Pose getPose( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.pose; -} - -template <> -inline double getLongitudinalVelocity( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - return p.point.longitudinal_velocity_mps; -} - -template <> -inline void setPose( - const geometry_msgs::msg::Pose & pose, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - p.point.pose = pose; -} - -template <> -inline void setLongitudinalVelocity( - const double velocity, autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) -{ - p.point.longitudinal_velocity_mps = velocity; -} -} // namespace tier4_autoware_utils - -#endif // TIER4_AUTOWARE_UTILS__GEOMETRY__PATH_WITH_LANE_ID_GEOMETRY_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp index 32b4ba663f..dfa678eaa0 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/pose_deviation.hpp @@ -15,10 +15,8 @@ #ifndef TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ #define TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" - -#include +#include +#include namespace tier4_autoware_utils { @@ -29,57 +27,18 @@ struct PoseDeviation double yaw{0.0}; }; -inline double calcLateralDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) -{ - const auto & base_point = base_pose.position; - - const auto yaw = tf2::getYaw(base_pose.orientation); - const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; - - const auto dx = target_point.x - base_point.x; - const auto dy = target_point.y - base_point.y; - const Eigen::Vector3d diff_vec{dx, dy, 0}; +double calcLateralDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point); - const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec); - - return cross_vec.z(); -} - -inline double calcLongitudinalDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) -{ - const auto & base_point = base_pose.position; +double calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point); - const auto yaw = tf2::getYaw(base_pose.orientation); - const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; - - const auto dx = target_point.x - base_point.x; - const auto dy = target_point.y - base_point.y; - const Eigen::Vector3d diff_vec{dx, dy, 0}; - - return base_unit_vec.dot(diff_vec); -} - -inline double calcYawDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) -{ - const auto base_yaw = tf2::getYaw(base_pose.orientation); - const auto target_yaw = tf2::getYaw(target_pose.orientation); - return normalizeRadian(target_yaw - base_yaw); -} - -inline PoseDeviation calcPoseDeviation( - const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) -{ - PoseDeviation deviation{}; +double calcYawDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); - deviation.lateral = calcLateralDeviation(base_pose, target_pose.position); - deviation.longitudinal = calcLongitudinalDeviation(base_pose, target_pose.position); - deviation.yaw = calcYawDeviation(base_pose, target_pose); +PoseDeviation calcPoseDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose); - return deviation; -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__POSE_DEVIATION_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp index 78e7c5f928..1d72b49a55 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp @@ -15,7 +15,7 @@ #ifndef TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ #define TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ -#include +#include #include @@ -64,56 +64,19 @@ inline std_msgs::msg::ColorRGBA createMarkerColor(float r, float g, float b, flo return color; } -inline visualization_msgs::msg::Marker createDefaultMarker( +visualization_msgs::msg::Marker createDefaultMarker( const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, const int32_t type, const geometry_msgs::msg::Vector3 & scale, - const std_msgs::msg::ColorRGBA & color) -{ - visualization_msgs::msg::Marker marker; - - marker.header.frame_id = frame_id; - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.type = type; - marker.action = visualization_msgs::msg::Marker::ADD; - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - - marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); - marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); - marker.scale = scale; - marker.color = color; - marker.frame_locked = true; + const std_msgs::msg::ColorRGBA & color); - return marker; -} - -inline visualization_msgs::msg::Marker createDeletedDefaultMarker( - const rclcpp::Time & now, const std::string & ns, const int32_t id) -{ - visualization_msgs::msg::Marker marker; +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id); - marker.header.stamp = now; - marker.ns = ns; - marker.id = id; - marker.action = visualization_msgs::msg::Marker::DELETE; - - return marker; -} - -inline void appendMarkerArray( +void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time = {}) -{ - for (const auto & marker : additional_marker_array.markers) { - marker_array->markers.push_back(marker); + const boost::optional & current_time = {}); - if (current_time) { - marker_array->markers.back().header.stamp = current_time.get(); - } - } -} } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__ROS__MARKER_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp new file mode 100644 index 0000000000..b0b526e8c8 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/system/backtrace.hpp @@ -0,0 +1,53 @@ +// 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 TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ +#define TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ + +#include + +#include +#include +#include +#include + +namespace tier4_autoware_utils +{ + +inline void print_backtrace() +{ + constexpr size_t max_frames = 100; + void * addrlist[max_frames + 1]; + + int addrlen = backtrace(addrlist, sizeof(addrlist) / sizeof(void *)); + + if (addrlen == 0) { + return; + } + + char ** symbol_list = backtrace_symbols(addrlist, addrlen); + + std::stringstream ss; + ss << " ********** back trace **********" << std::endl; + for (int i = 1; i < addrlen; i++) { + ss << " @ " << symbol_list[i] << std::endl; + } + std::cerr << ss.str() << std::endl; + + free(symbol_list); +} + +} // namespace tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__SYSTEM__BACKTRACE_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp deleted file mode 100644 index 2aa5ae0c7e..0000000000 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ /dev/null @@ -1,44 +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 TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ -#define TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ - -#include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" -#include "tier4_autoware_utils/geometry/pose_deviation.hpp" -#include "tier4_autoware_utils/math/constants.hpp" -#include "tier4_autoware_utils/math/normalization.hpp" -#include "tier4_autoware_utils/math/range.hpp" -#include "tier4_autoware_utils/math/sin_table.hpp" -#include "tier4_autoware_utils/math/trigonometry.hpp" -#include "tier4_autoware_utils/math/unit_conversion.hpp" -#include "tier4_autoware_utils/ros/debug_publisher.hpp" -#include "tier4_autoware_utils/ros/debug_traits.hpp" -#include "tier4_autoware_utils/ros/marker_helper.hpp" -#include "tier4_autoware_utils/ros/msg_covariance.hpp" -#include "tier4_autoware_utils/ros/msg_operation.hpp" -#include "tier4_autoware_utils/ros/parameter.hpp" -#include "tier4_autoware_utils/ros/processing_time_publisher.hpp" -#include "tier4_autoware_utils/ros/self_pose_listener.hpp" -#include "tier4_autoware_utils/ros/transform_listener.hpp" -#include "tier4_autoware_utils/ros/update_param.hpp" -#include "tier4_autoware_utils/ros/uuid_helper.hpp" -#include "tier4_autoware_utils/ros/wait_for_param.hpp" -#include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_autoware_utils/transform/transforms.hpp" - -#endif // TIER4_AUTOWARE_UTILS__TIER4_AUTOWARE_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index fd9b315f8e..6a6b3812cd 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Takayuki Murooka Yutaka Shimizu + Mamoru Sobue Apache License 2.0 ament_cmake_auto diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index c212eb269d..79c9f9937c 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -16,6 +16,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" +#include + #include namespace diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp new file mode 100644 index 0000000000..2c2de2d07a --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/geometry.cpp @@ -0,0 +1,384 @@ +// 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 "tier4_autoware_utils/geometry/geometry.hpp" + +#include + +#include + +#include + +namespace tf2 +{ +void fromMsg(const geometry_msgs::msg::PoseStamped & msg, tf2::Stamped & out) +{ + out.stamp_ = tf2_ros::fromMsg(msg.header.stamp); + out.frame_id_ = msg.header.frame_id; + tf2::Transform tmp; + fromMsg(msg.pose, tmp); + out.setData(tmp); +} +} // namespace tf2 + +namespace tier4_autoware_utils +{ +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Quaternion & quat) +{ + geometry_msgs::msg::Vector3 rpy; + tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); + tf2::Matrix3x3(q).getRPY(rpy.x, rpy.y, rpy.z); + return rpy; +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::Pose & pose) +{ + return getRPY(pose.orientation); +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseStamped & pose) +{ + return getRPY(pose.pose); +} + +geometry_msgs::msg::Vector3 getRPY(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + return getRPY(pose.pose.pose); +} + +geometry_msgs::msg::Quaternion createQuaternion( + const double x, const double y, const double z, const double w) +{ + geometry_msgs::msg::Quaternion q; + q.x = x; + q.y = y; + q.z = z; + q.w = w; + return q; +} + +geometry_msgs::msg::Vector3 createTranslation(const double x, const double y, const double z) +{ + geometry_msgs::msg::Vector3 v; + v.x = x; + v.y = y; + v.z = z; + return v; +} + +// Revival of tf::createQuaternionFromRPY +// https://answers.ros.org/question/304397/recommended-way-to-construct-quaternion-from-rollpitchyaw-with-tf2/ +geometry_msgs::msg::Quaternion createQuaternionFromRPY( + const double roll, const double pitch, const double yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} + +geometry_msgs::msg::Quaternion createQuaternionFromYaw(const double yaw) +{ + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + return tf2::toMsg(q); +} + +double calcElevationAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dz = p_to.z - p_from.z; + const double dist_2d = calcDistance2d(p_from, p_to); + return std::atan2(dz, dist_2d); +} + +/** + * @brief calculate azimuth angle of two points. + * @details This function returns the azimuth angle of the position of the two input points + * with respect to the origin of their coordinate system. + * If x and y of the two points are the same, the calculation result will be unstable. + * @param p_from source point + * @param p_to target point + * @return -pi < azimuth angle < pi. + */ +double calcAzimuthAngle( + const geometry_msgs::msg::Point & p_from, const geometry_msgs::msg::Point & p_to) +{ + const double dx = p_to.x - p_from.x; + const double dy = p_to.y - p_from.y; + return std::atan2(dy, dx); +} + +geometry_msgs::msg::Pose transform2pose(const geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = transform.translation.x; + pose.position.y = transform.translation.y; + pose.position.z = transform.translation.z; + pose.orientation = transform.rotation; + return pose; +} + +geometry_msgs::msg::PoseStamped transform2pose( + const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::PoseStamped pose; + pose.header = transform.header; + pose.pose = transform2pose(transform.transform); + return pose; +} + +geometry_msgs::msg::Transform pose2transform(const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = pose.position.x; + transform.translation.y = pose.position.y; + transform.translation.z = pose.position.z; + transform.rotation = pose.orientation; + return transform; +} + +geometry_msgs::msg::TransformStamped pose2transform( + const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame_id) +{ + geometry_msgs::msg::TransformStamped transform; + transform.header = pose.header; + transform.transform = pose2transform(pose.pose); + transform.child_frame_id = child_frame_id; + return transform; +} + +Point3d transformPoint(const Point3d & point, const geometry_msgs::msg::Transform & transform) +{ + const auto & translation = transform.translation; + const auto & rotation = transform.rotation; + + const Eigen::Translation3d T(translation.x, translation.y, translation.z); + const Eigen::Quaterniond R(rotation.w, rotation.x, rotation.y, rotation.z); + + const Eigen::Vector3d transformed(T * R * point); + + return Point3d{transformed.x(), transformed.y(), transformed.z()}; +} + +Point2d transformPoint(const Point2d & point, const geometry_msgs::msg::Transform & transform) +{ + Point3d point_3d{point.x(), point.y(), 0}; + const auto transformed = transformPoint(point_3d, transform); + return Point2d{transformed.x(), transformed.y()}; +} + +Eigen::Vector3d transformPoint(const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) +{ + geometry_msgs::msg::Transform transform; + transform.translation.x = pose.position.x; + transform.translation.y = pose.position.y; + transform.translation.z = pose.position.z; + transform.rotation = pose.orientation; + + Point3d p = transformPoint(Point3d(point.x(), point.y(), point.z()), transform); + return Eigen::Vector3d(p.x(), p.y(), p.z()); +} + +geometry_msgs::msg::Point transformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Vector3d vec = Eigen::Vector3d(point.x, point.y, point.z); + auto transformed_vec = transformPoint(vec, pose); + + geometry_msgs::msg::Point transformed_point; + transformed_point.x = transformed_vec.x(); + transformed_point.y = transformed_vec.y(); + transformed_point.z = transformed_vec.z(); + return transformed_point; +} + +geometry_msgs::msg::Point32 transformPoint( + const geometry_msgs::msg::Point32 & point32, const geometry_msgs::msg::Pose & pose) +{ + const auto point = + geometry_msgs::build().x(point32.x).y(point32.y).z(point32.z); + const auto transformed_point = tier4_autoware_utils::transformPoint(point, pose); + return geometry_msgs::build() + .x(transformed_point.x) + .y(transformed_point.y) + .z(transformed_point.z); +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) +{ + geometry_msgs::msg::Pose transformed_pose; + tf2::doTransform(pose, transformed_pose, transform); + + return transformed_pose; +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, geometry_msgs::msg::Transform & transform) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform = transform; + + return transformPose(pose, transform_stamped); +} + +geometry_msgs::msg::Pose transformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & pose_transform) +{ + tf2::Transform transform; + tf2::convert(pose_transform, transform); + + geometry_msgs::msg::TransformStamped transform_msg; + transform_msg.transform = tf2::toMsg(transform); + + return transformPose(pose, transform_msg); +} + +// Transform pose in world coordinates to local coordinates +/* +geometry_msgs::msg::Pose inverseTransformPose( +const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::TransformStamped & transform) +{ +tf2::Transform tf; +tf2::fromMsg(transform, tf); +geometry_msgs::msg::TransformStamped transform_stamped; +transform_stamped.transform = tf2::toMsg(tf.inverse()); + +return transformPose(pose, transform_stamped); +} +*/ + +// Transform pose in world coordinates to local coordinates +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Transform & transform) +{ + tf2::Transform tf; + tf2::fromMsg(transform, tf); + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.transform = tf2::toMsg(tf.inverse()); + + return transformPose(pose, transform_stamped); +} + +// Transform pose in world coordinates to local coordinates +geometry_msgs::msg::Pose inverseTransformPose( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Pose & transform_pose) +{ + tf2::Transform transform; + tf2::convert(transform_pose, transform); + + return inverseTransformPose(pose, tf2::toMsg(transform)); +} + +// Transform point in world coordinates to local coordinates +Eigen::Vector3d inverseTransformPoint( + const Eigen::Vector3d & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Quaterniond q( + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z); + const Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + + const Eigen::Vector3d local_origin(pose.position.x, pose.position.y, pose.position.z); + Eigen::Vector3d local_point = R.transpose() * point - R.transpose() * local_origin; + + return local_point; +} + +// Transform point in world coordinates to local coordinates +geometry_msgs::msg::Point inverseTransformPoint( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & pose) +{ + const Eigen::Vector3d local_vec = + inverseTransformPoint(Eigen::Vector3d(point.x, point.y, point.z), pose); + geometry_msgs::msg::Point local_point; + local_point.x = local_vec.x(); + local_point.y = local_vec.y(); + local_point.z = local_vec.z(); + return local_point; +} + +double calcCurvature( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3) +{ + // Calculation details are described in the following page + // https://en.wikipedia.org/wiki/Menger_curvature + const double denominator = + calcDistance2d(p1, p2) * calcDistance2d(p2, p3) * calcDistance2d(p3, p1); + if (std::fabs(denominator) < 1e-10) { + throw std::runtime_error("points are too close for curvature calculation."); + } + return 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / denominator; +} + +/** + * @brief Calculate offset pose. The offset values are defined in the local coordinate of the input + * pose. + */ +geometry_msgs::msg::Pose calcOffsetPose( + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::Transform transform; + transform.translation = createTranslation(x, y, z); + transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); + tf2::Transform tf_pose; + tf2::Transform tf_offset; + tf2::fromMsg(transform, tf_offset); + tf2::fromMsg(p, tf_pose); + tf2::toMsg(tf_pose * tf_offset, pose); + return pose; +} + +/** + * @brief Judge whether twist covariance is valid. + * + * @param twist_with_covariance source twist with covariance + * @return If all element of covariance is 0, return false. + */ +// +bool isTwistCovarianceValid(const geometry_msgs::msg::TwistWithCovariance & twist_with_covariance) +{ + for (const auto & c : twist_with_covariance.covariance) { + if (c != 0.0) { + return true; + } + } + return false; +} + +// NOTE: much faster than boost::geometry::intersects() +std::optional intersect( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + // calculate intersection point + const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y); + if (det == 0.0) { + return std::nullopt; + } + + const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det; + const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det; + if (t < 0 || 1 < t || s < 0 || 1 < s) { + return std::nullopt; + } + + geometry_msgs::msg::Point intersect_point; + intersect_point.x = t * p1.x + (1.0 - t) * p2.x; + intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + return intersect_point; +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp b/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp new file mode 100644 index 0000000000..dda8afb974 --- /dev/null +++ b/common/tier4_autoware_utils/src/geometry/pose_deviation.cpp @@ -0,0 +1,85 @@ +// 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 "tier4_autoware_utils/geometry/pose_deviation.hpp" + +#include "tier4_autoware_utils/math/normalization.hpp" + +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +namespace tier4_autoware_utils +{ + +double calcLateralDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + const Eigen::Vector3d cross_vec = base_unit_vec.cross(diff_vec); + + return cross_vec.z(); +} + +double calcLongitudinalDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Point & target_point) +{ + const auto & base_point = base_pose.position; + + const auto yaw = tf2::getYaw(base_pose.orientation); + const Eigen::Vector3d base_unit_vec{std::cos(yaw), std::sin(yaw), 0}; + + const auto dx = target_point.x - base_point.x; + const auto dy = target_point.y - base_point.y; + const Eigen::Vector3d diff_vec{dx, dy, 0}; + + return base_unit_vec.dot(diff_vec); +} + +double calcYawDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + const auto base_yaw = tf2::getYaw(base_pose.orientation); + const auto target_yaw = tf2::getYaw(target_pose.orientation); + return normalizeRadian(target_yaw - base_yaw); +} + +PoseDeviation calcPoseDeviation( + const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & target_pose) +{ + PoseDeviation deviation{}; + + deviation.lateral = calcLateralDeviation(base_pose, target_pose.position); + deviation.longitudinal = calcLongitudinalDeviation(base_pose, target_pose.position); + deviation.yaw = calcYawDeviation(base_pose, target_pose); + + return deviation; +} +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/tier4_autoware_utils/src/ros/marker_helper.cpp new file mode 100644 index 0000000000..fe3a579e41 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/marker_helper.cpp @@ -0,0 +1,71 @@ +// 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 "tier4_autoware_utils/ros/marker_helper.hpp" + +namespace tier4_autoware_utils +{ + +visualization_msgs::msg::Marker createDefaultMarker( + const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id, + const int32_t type, const geometry_msgs::msg::Vector3 & scale, + const std_msgs::msg::ColorRGBA & color) +{ + visualization_msgs::msg::Marker marker; + + marker.header.frame_id = frame_id; + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.type = type; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0); + marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0); + marker.scale = scale; + marker.color = color; + marker.frame_locked = true; + + return marker; +} + +visualization_msgs::msg::Marker createDeletedDefaultMarker( + const rclcpp::Time & now, const std::string & ns, const int32_t id) +{ + visualization_msgs::msg::Marker marker; + + marker.header.stamp = now; + marker.ns = ns; + marker.id = id; + marker.action = visualization_msgs::msg::Marker::DELETE; + + return marker; +} + +void appendMarkerArray( + const visualization_msgs::msg::MarkerArray & additional_marker_array, + visualization_msgs::msg::MarkerArray * marker_array, + const boost::optional & current_time) +{ + for (const auto & marker : additional_marker_array.markers) { + marker_array->markers.push_back(marker); + + if (current_time) { + marker_array->markers.back().header.stamp = current_time.get(); + } + } +} + +} // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp index d6e35ea5a8..a8179f3b3c 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_path_with_lane_id_geometry.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include diff --git a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp index 383c35921c..76e742edde 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_pose_deviation.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/geometry/pose_deviation.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" diff --git a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp index 34018ed730..4ecfcb6bee 100644 --- a/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp +++ b/common/tier4_autoware_utils/test/src/test_autoware_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include #include diff --git a/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000..f86b6d4d2e --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/CMakeLists.txt @@ -0,0 +1,27 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_logging_level_configure_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(tier4_logging_level_configure_rviz_plugin SHARED + include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp + src/logging_level_configure.cpp +) + +target_link_libraries(tier4_logging_level_configure_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_logging_level_configure_rviz_plugin/README.md b/common/tier4_logging_level_configure_rviz_plugin/README.md new file mode 100644 index 0000000000..c3523ed802 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/README.md @@ -0,0 +1,5 @@ +# tier4_logging_level_configure_rviz_plugin + +This package provides an rviz_plugin that can easily change the logger level of each node + +![tier4_logging_level_configure_rviz_plugin](tier4_logging_level_configure_rviz_plugin.png) diff --git a/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp new file mode 100644 index 0000000000..f513a7e04a --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/include/tier4_logging_level_configure_rviz_plugin/logging_level_configure.hpp @@ -0,0 +1,74 @@ +// 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 TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ +#define TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ + +#include "logging_demo/srv/config_logger.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugin +{ + +class LoggingLevelConfigureRvizPlugin : public rviz_common::Panel +{ + Q_OBJECT // This macro is needed for Qt to handle slots and signals + + public : LoggingLevelConfigureRvizPlugin(QWidget * parent = nullptr); + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config & config) override; + +private: + QMap buttonGroups_; + rclcpp::Node::SharedPtr raw_node_; + + // logger_node_map_[target_name] = {container_name, logger_name} + std::map>> logger_node_map_; + + // client_map_[container_name] = service_client + std::unordered_map::SharedPtr> + client_map_; + + // button_map_[target_name][logging_level] = Q_button_pointer + std::unordered_map> button_map_; + + QStringList getContainerList(); + int getMaxModuleNameWidth(QLabel * containerLabel); + void setLoggerNodeMap(); + void attachLoggingComponent(); + +private Q_SLOTS: + void onButtonClick(QPushButton * button, const QString & name, const QString & level); + void updateButtonColors(const QString & target_module_name, QPushButton * active_button); + void changeLogLevel(const QString & container, const QString & level); +}; + +} // namespace rviz_plugin + +#endif // TIER4_LOGGING_LEVEL_CONFIGURE_RVIZ_PLUGIN__LOGGING_LEVEL_CONFIGURE_HPP_ diff --git a/common/tier4_logging_level_configure_rviz_plugin/package.xml b/common/tier4_logging_level_configure_rviz_plugin/package.xml new file mode 100644 index 0000000000..4ba5bb1579 --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/package.xml @@ -0,0 +1,32 @@ + + + + tier4_logging_level_configure_rviz_plugin + 0.1.0 + The tier4_logging_level_configure_rviz_plugin package + Takamasa Horibe + Satoshi Ota + Kosuke Takeuchi + Apache License 2.0 + + ament_cmake + autoware_cmake + + libqt5-core + libqt5-gui + libqt5-widgets + logging_demo + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + rviz_rendering + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000..ce5cbd13fc --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,7 @@ + + + tier4_logging_level_configure_rviz_plugin + + diff --git a/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp new file mode 100644 index 0000000000..e645ff682d --- /dev/null +++ b/common/tier4_logging_level_configure_rviz_plugin/src/logging_level_configure.cpp @@ -0,0 +1,250 @@ +// 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 +#include +#include + +#include + +namespace rviz_plugin +{ + +LoggingLevelConfigureRvizPlugin::LoggingLevelConfigureRvizPlugin(QWidget * parent) +: rviz_common::Panel(parent) +{ +} + +// Calculate the maximum width among all target_module_name. +int LoggingLevelConfigureRvizPlugin::getMaxModuleNameWidth(QLabel * containerLabel) +{ + int max_width = 0; + QFontMetrics metrics(containerLabel->font()); + for (const auto & item : logger_node_map_) { + const auto & target_module_name = item.first; + int width = metrics.horizontalAdvance(target_module_name); + if (width > max_width) { + max_width = width; + } + } + return max_width; +} + +// create container list in logger_node_map_ without +QStringList LoggingLevelConfigureRvizPlugin::getContainerList() +{ + QStringList containers; + for (const auto & item : logger_node_map_) { + const auto & container_logger_vec = item.second; + for (const auto & container_logger_pair : container_logger_vec) { + if (!containers.contains(container_logger_pair.first)) { + containers.append(container_logger_pair.first); + } + } + } + return containers; +} + +void LoggingLevelConfigureRvizPlugin::onInitialize() +{ + setLoggerNodeMap(); + + attachLoggingComponent(); + + QVBoxLayout * layout = new QVBoxLayout; + + QStringList levels = {"DEBUG", "INFO", "WARN", "ERROR", "FATAL"}; + + QStringList loaded_container; + constexpr int height = 20; + for (const auto & item : logger_node_map_) { + const auto & target_module_name = item.first; + + QHBoxLayout * hLayout = new QHBoxLayout; + + // Create a QLabel to display the container name. + QLabel * containerLabel = new QLabel(target_module_name); + containerLabel->setFixedHeight(height); // Set fixed height for the button + containerLabel->setFixedWidth(getMaxModuleNameWidth(containerLabel)); + + hLayout->addWidget(containerLabel); // Add the QLabel to the hLayout. + + QButtonGroup * group = new QButtonGroup(this); + for (const QString & level : levels) { + QPushButton * btn = new QPushButton(level); + btn->setFixedHeight(height); // Set fixed height for the button + hLayout->addWidget(btn); // Add each QPushButton to the hLayout. + group->addButton(btn); + button_map_[target_module_name][level] = btn; + connect(btn, &QPushButton::clicked, this, [this, btn, target_module_name, level]() { + this->onButtonClick(btn, target_module_name, level); + }); + } + // Set the "INFO" button as checked by default and change its color. + updateButtonColors(target_module_name, button_map_[target_module_name]["INFO"]); + + buttonGroups_[target_module_name] = group; + layout->addLayout(hLayout); + } + + setLayout(layout); + + // set up service clients + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + const auto & containers = getContainerList(); + for (const QString & container : containers) { + const auto client = raw_node_->create_client( + container.toStdString() + "/config_logger"); + client_map_[container] = client; + } +} + +void LoggingLevelConfigureRvizPlugin::attachLoggingComponent() +{ + const auto & containers = getContainerList(); + for (const auto & container_name : containers) { + // Load the component for each container + QString command = "ros2 component load --node-namespace " + container_name + + " --node-name logging_configure " + container_name + + " logging_demo logging_demo::LoggerConfig"; + int result = system(qPrintable(command)); + std::cerr << "load logger in " << container_name.toStdString() << ": result = " << result + << std::endl; + } +} + +// Modify the signature of the onButtonClick function: +void LoggingLevelConfigureRvizPlugin::onButtonClick( + QPushButton * button, const QString & target_module_name, const QString & level) +{ + if (button) { + const auto callback = + [&](rclcpp::Client::SharedFuture future) { + std::cerr << "change logging level: " + << std::string(future.get()->success ? "success!" : "failed...") << std::endl; + }; + + for (const auto & container_logger_map : logger_node_map_[target_module_name]) { + const auto container_node = container_logger_map.first; + const auto logger_name = container_logger_map.second; + const auto req = std::make_shared(); + + req->logger_name = logger_name.toStdString(); + req->level = level.toStdString(); + std::cerr << "logger level of " << req->logger_name << " is set to " << req->level + << std::endl; + client_map_[container_node]->async_send_request(req, callback); + } + + updateButtonColors( + target_module_name, button); // Modify updateButtonColors to accept QPushButton only. + } +} + +void LoggingLevelConfigureRvizPlugin::updateButtonColors( + const QString & target_module_name, QPushButton * active_button) +{ + const QString LIGHT_GREEN = "rgb(181, 255, 20)"; + const QString LIGHT_GRAY = "rgb(211, 211, 211)"; + const QString LIGHT_GRAY_TEXT = "rgb(180, 180, 180)"; + + for (const auto & button : button_map_[target_module_name]) { + if (button.second == active_button) { + button.second->setStyleSheet("background-color: " + LIGHT_GREEN + "; color: black"); + } else { + button.second->setStyleSheet( + "background-color: " + LIGHT_GRAY + "; color: " + LIGHT_GRAY_TEXT); + } + } +} + +void LoggingLevelConfigureRvizPlugin::save(rviz_common::Config config) const +{ + Panel::save(config); +} + +void LoggingLevelConfigureRvizPlugin::load(const rviz_common::Config & config) +{ + Panel::load(config); +} + +void LoggingLevelConfigureRvizPlugin::setLoggerNodeMap() +{ + // =============================================================================================== + // ====================================== Planning =============================================== + // =============================================================================================== + + QString behavior_planning_container = + "/planning/scenario_planning/lane_driving/behavior_planning/behavior_planning_container"; + QString motion_planning_container = + "/planning/scenario_planning/lane_driving/motion_planning/motion_planning_container"; + + // behavior_path_planner (all) + logger_node_map_["behavior_path_planner"] = { + {behavior_planning_container, + "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner"}, + {behavior_planning_container, "tier4_autoware_utils"}}; + + // behavior_path_planner: avoidance + logger_node_map_["behavior_path_planner: avoidance"] = { + {behavior_planning_container, + "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner." + "avoidance"}}; + + // behavior_velocity_planner (all) + logger_node_map_["behavior_velocity_planner"] = { + {behavior_planning_container, + "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner"}, + {behavior_planning_container, "tier4_autoware_utils"}}; + + // behavior_velocity_planner: intersection + logger_node_map_["behavior_velocity_planner: intersection"] = { + {behavior_planning_container, + "planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner." + "intersection"}}; + + // obstacle_avoidance_planner + logger_node_map_["motion: obstacle_avoidance"] = { + {motion_planning_container, + "planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner"}, + {motion_planning_container, "tier4_autoware_utils"}}; + + // motion_velocity_smoother + QString container = "/planning/scenario_planning/motion_velocity_smoother_container"; + logger_node_map_["motion: velocity_smoother"] = { + {container, "planning.scenario_planning.motion_velocity_smoother"}, + {container, "tier4_autoware_utils"}}; + + // =============================================================================================== + // ======================================= Control =============================================== + // =============================================================================================== + + QString control_container = "/control/control_container"; + + // lateral_controller + logger_node_map_["lateral_controller"] = { + {control_container, "control.trajectory_follower.controller_node_exe.lateral_controller"}, + {control_container, "tier4_autoware_utils"}, + }; + + // longitudinal_controller + logger_node_map_["longitudinal_controller"] = { + {control_container, "control.trajectory_follower.controller_node_exe.longitudinal_controller"}, + {control_container, "tier4_autoware_utils"}, + }; +} + +} // namespace rviz_plugin +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugin::LoggingLevelConfigureRvizPlugin, rviz_common::Panel) diff --git a/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png new file mode 100644 index 0000000000..a93aff724b Binary files /dev/null and b/common/tier4_logging_level_configure_rviz_plugin/tier4_logging_level_configure_rviz_plugin.png differ diff --git a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp index ccedcceefa..74c2a60df3 100644 --- a/common/tier4_planning_rviz_plugin/include/path/display_base.hpp +++ b/common/tier4_planning_rviz_plugin/include/path/display_base.hpp @@ -133,7 +133,10 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay 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_} + property_point_offset_{"Offset", 0.0, "", &property_point_view_}, + // slope + property_slope_text_view_{"View Text Slope", false, "", this}, + property_slope_text_scale_{"Scale", 0.3, "", &property_slope_text_view_} { // path property_path_width_.setMin(0.0); @@ -171,6 +174,12 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay node->detachAllObjects(); this->scene_manager_->destroySceneNode(node); } + for (size_t i = 0; i < slope_text_nodes_.size(); i++) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->removeAndDestroyAllChildren(); + node->detachAllObjects(); + this->scene_manager_->destroySceneNode(node); + } this->scene_manager_->destroyManualObject(footprint_manual_object_); this->scene_manager_->destroyManualObject(point_manual_object_); } @@ -198,6 +207,7 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay 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(); @@ -206,6 +216,16 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay } velocity_text_nodes_.clear(); velocity_texts_.clear(); + + for (size_t i = 0; i < slope_texts_.size(); i++) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + slope_text_nodes_.clear(); + slope_texts_.clear(); + footprint_manual_object_->clear(); point_manual_object_->clear(); } @@ -300,6 +320,29 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay velocity_text_nodes_.resize(msg_ptr->points.size()); } + if (msg_ptr->points.size() > slope_texts_.size()) { + for (size_t i = slope_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); + slope_texts_.push_back(text); + slope_text_nodes_.push_back(node); + } + } else if (msg_ptr->points.size() < slope_texts_.size()) { + for (size_t i = slope_texts_.size() - 1; i >= msg_ptr->points.size(); i--) { + Ogre::SceneNode * node = slope_text_nodes_.at(i); + node->detachAllObjects(); + node->removeAndDestroyAllChildren(); + this->scene_manager_->destroySceneNode(node); + } + slope_texts_.resize(msg_ptr->points.size()); + slope_text_nodes_.resize(msg_ptr->points.size()); + } + const auto info = vehicle_footprint_info_; const float left = property_path_width_view_.getBool() ? -property_path_width_.getFloat() / 2.0 : -info->width / 2.0; @@ -395,6 +438,38 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_rendering::MovableText * text = velocity_texts_.at(point_idx); text->setVisible(false); } + + // slope text + if (property_slope_text_view_.getBool() && 1 < msg_ptr->points.size()) { + const size_t prev_idx = + (point_idx != msg_ptr->points.size() - 1) ? point_idx : point_idx - 1; + const size_t next_idx = + (point_idx != msg_ptr->points.size() - 1) ? point_idx + 1 : point_idx; + + const auto & prev_path_pos = + tier4_autoware_utils::getPose(msg_ptr->points.at(prev_idx)).position; + const auto & next_path_pos = + tier4_autoware_utils::getPose(msg_ptr->points.at(next_idx)).position; + + Ogre::Vector3 position; + position.x = pose.position.x; + position.y = pose.position.y; + position.z = pose.position.z; + Ogre::SceneNode * node = slope_text_nodes_.at(point_idx); + node->setPosition(position); + + rviz_rendering::MovableText * text = slope_texts_.at(point_idx); + const double slope = tier4_autoware_utils::calcElevationAngle(prev_path_pos, next_path_pos); + + std::stringstream ss; + ss << std::fixed << std::setprecision(2) << slope; + text->setCaption(ss.str()); + text->setCharacterHeight(property_slope_text_scale_.getFloat()); + text->setVisible(true); + } else { + rviz_rendering::MovableText * text = slope_texts_.at(point_idx); + text->setVisible(false); + } } path_manual_object_->end(); @@ -526,6 +601,8 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay Ogre::ManualObject * point_manual_object_{nullptr}; std::vector velocity_texts_; std::vector velocity_text_nodes_; + std::vector slope_texts_; + std::vector slope_text_nodes_; rviz_common::properties::BoolProperty property_path_view_; rviz_common::properties::BoolProperty property_path_width_view_; @@ -556,6 +633,9 @@ class AutowarePathBaseDisplay : public rviz_common::MessageFilterDisplay rviz_common::properties::FloatProperty property_point_radius_; rviz_common::properties::FloatProperty property_point_offset_; + rviz_common::properties::BoolProperty property_slope_text_view_; + rviz_common::properties::FloatProperty property_slope_text_scale_; + std::shared_ptr vehicle_info_; private: diff --git a/common/tier4_planning_rviz_plugin/include/utils.hpp b/common/tier4_planning_rviz_plugin/include/utils.hpp index 8a16059cbf..76f9da0685 100644 --- a/common/tier4_planning_rviz_plugin/include/utils.hpp +++ b/common/tier4_planning_rviz_plugin/include/utils.hpp @@ -16,7 +16,6 @@ #define UTILS_HPP_ #include -#include #include #include diff --git a/common/tier4_system_rviz_plugin/CMakeLists.txt b/common/tier4_system_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000..a65379c553 --- /dev/null +++ b/common/tier4_system_rviz_plugin/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_system_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) + +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +set(HEADERS + src/mrm_summary_overlay_display.hpp + src/jsk_overlay_utils.hpp +) + +## Declare a C++ library +ament_auto_add_library(tier4_system_rviz_plugin SHARED + src/mrm_summary_overlay_display.cpp + src/jsk_overlay_utils.cpp + ${HEADERS} +) + +target_link_libraries(tier4_system_rviz_plugin + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + plugins +) diff --git a/common/tier4_system_rviz_plugin/README.md b/common/tier4_system_rviz_plugin/README.md new file mode 100644 index 0000000000..098844c8b4 --- /dev/null +++ b/common/tier4_system_rviz_plugin/README.md @@ -0,0 +1 @@ +# tier4_system_rviz_plugin diff --git a/common/tier4_system_rviz_plugin/package.xml b/common/tier4_system_rviz_plugin/package.xml new file mode 100644 index 0000000000..adae997ea0 --- /dev/null +++ b/common/tier4_system_rviz_plugin/package.xml @@ -0,0 +1,30 @@ + + + + tier4_system_rviz_plugin + 0.1.0 + The tier4_vehicle_rviz_plugin package + Koji Minoda + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_system_msgs + diagnostic_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rviz_common + rviz_default_plugins + rviz_ogre_vendor + tier4_autoware_utils + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/common/tier4_system_rviz_plugin/plugins/plugin_description.xml b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000..a3ac26d1a3 --- /dev/null +++ b/common/tier4_system_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,6 @@ + + + + diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp new file mode 100644 index 0000000000..36c5d1ce82 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.cpp @@ -0,0 +1,222 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "jsk_overlay_utils.hpp" + +#include + +namespace jsk_rviz_plugins +{ +ScopedPixelBuffer::ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer) +: pixel_buffer_(pixel_buffer) +{ + pixel_buffer_->lock(Ogre::HardwareBuffer::HBL_NORMAL); +} + +ScopedPixelBuffer::~ScopedPixelBuffer() +{ + pixel_buffer_->unlock(); +} + +Ogre::HardwarePixelBufferSharedPtr ScopedPixelBuffer::getPixelBuffer() +{ + return pixel_buffer_; +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height) +{ + const Ogre::PixelBox & pixelBox = pixel_buffer_->getCurrentLock(); + Ogre::uint8 * pDest = static_cast(pixelBox.data); + memset(pDest, 0, width * height); + return QImage(pDest, width, height, QImage::Format_ARGB32); +} + +QImage ScopedPixelBuffer::getQImage(unsigned int width, unsigned int height, QColor & bg_color) +{ + QImage Hud = getQImage(width, height); + for (unsigned int i = 0; i < width; i++) { + for (unsigned int j = 0; j < height; j++) { + Hud.setPixel(i, j, bg_color.rgba()); + } + } + return Hud; +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight()); +} + +QImage ScopedPixelBuffer::getQImage(OverlayObject & overlay, QColor & bg_color) +{ + return getQImage(overlay.getTextureWidth(), overlay.getTextureHeight(), bg_color); +} + +OverlayObject::OverlayObject(const std::string & name) : name_(name) +{ + std::string material_name = name_ + "Material"; + Ogre::OverlayManager * mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + overlay_ = mOverlayMgr->create(name_); + panel_ = static_cast( + mOverlayMgr->createOverlayElement("Panel", name_ + "Panel")); + panel_->setMetricsMode(Ogre::GMM_PIXELS); + + panel_material_ = Ogre::MaterialManager::getSingleton().create( + material_name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + panel_->setMaterialName(panel_material_->getName()); + overlay_->add2D(panel_); +} + +OverlayObject::~OverlayObject() +{ + hide(); + panel_material_->unload(); + Ogre::MaterialManager::getSingleton().remove(panel_material_->getName()); + // Ogre::OverlayManager* mOverlayMgr = Ogre::OverlayManager::getSingletonPtr(); + // mOverlayMgr->destroyOverlayElement(panel_); + // delete panel_; + // delete overlay_; +} + +std::string OverlayObject::getName() +{ + return name_; +} + +void OverlayObject::hide() +{ + if (overlay_->isVisible()) { + overlay_->hide(); + } +} + +void OverlayObject::show() +{ + if (!overlay_->isVisible()) { + overlay_->show(); + } +} + +bool OverlayObject::isTextureReady() +{ + return static_cast(texture_); +} + +void OverlayObject::updateTextureSize(unsigned int width, unsigned int height) +{ + const std::string texture_name = name_ + "Texture"; + if (width == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "width=0 is specified as texture size"); + width = 1; + } + if (height == 0) { + RCLCPP_WARN(rclcpp::get_logger("OverlayObject"), "height=0 is specified as texture size"); + height = 1; + } + if (!isTextureReady() || ((width != texture_->getWidth()) || (height != texture_->getHeight()))) { + if (isTextureReady()) { + Ogre::TextureManager::getSingleton().remove(texture_name); + panel_material_->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); + } + texture_ = Ogre::TextureManager::getSingleton().createManual( + texture_name, // name + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, // type + width, height, // width & height of the render window + 0, // number of mipmaps + Ogre::PF_A8R8G8B8, // pixel format chosen to match a format Qt can use + Ogre::TU_DEFAULT // usage + ); + panel_material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_name); + + panel_material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + } +} + +ScopedPixelBuffer OverlayObject::getBuffer() +{ + if (isTextureReady()) { + return ScopedPixelBuffer(texture_->getBuffer()); + } else { + return ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr()); + } +} + +void OverlayObject::setPosition(double left, double top) +{ + panel_->setPosition(left, top); +} + +void OverlayObject::setDimensions(double width, double height) +{ + panel_->setDimensions(width, height); +} + +bool OverlayObject::isVisible() +{ + return overlay_->isVisible(); +} + +unsigned int OverlayObject::getTextureWidth() +{ + if (isTextureReady()) { + return texture_->getWidth(); + } else { + return 0; + } +} + +unsigned int OverlayObject::getTextureHeight() +{ + if (isTextureReady()) { + return texture_->getHeight(); + } else { + return 0; + } +} + +} // namespace jsk_rviz_plugins diff --git a/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp new file mode 100644 index 0000000000..568111f241 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/jsk_overlay_utils.hpp @@ -0,0 +1,138 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef JSK_OVERLAY_UTILS_HPP_ +#define JSK_OVERLAY_UTILS_HPP_ + +#include +#include +#include +#include +#include + +#include +#include +// see OGRE/OgrePrerequisites.h +// #define OGRE_VERSION +// ((OGRE_VERSION_MAJOR << 16) | (OGRE_VERSION_MINOR << 8) | OGRE_VERSION_PATCH) +#if OGRE_VERSION < ((1 << 16) | (9 << 8) | 0) +#include +#include +#include +#include +#else +#include +#include +#include +#include +#include +#endif + +#include +#include +#include + +namespace jsk_rviz_plugins +{ +class OverlayObject; + +class ScopedPixelBuffer +{ +public: + explicit ScopedPixelBuffer(Ogre::HardwarePixelBufferSharedPtr pixel_buffer); + virtual ~ScopedPixelBuffer(); + virtual Ogre::HardwarePixelBufferSharedPtr getPixelBuffer(); + virtual QImage getQImage(unsigned int width, unsigned int height); + virtual QImage getQImage(OverlayObject & overlay); + virtual QImage getQImage(unsigned int width, unsigned int height, QColor & bg_color); + virtual QImage getQImage(OverlayObject & overlay, QColor & bg_color); + +protected: + Ogre::HardwarePixelBufferSharedPtr pixel_buffer_; + +private: +}; + +// this is a class for put overlay object on rviz 3D panel. +// This class suppose to be instantiated in onInitialize method +// of rviz::Display class. +class OverlayObject +{ +public: + typedef std::shared_ptr Ptr; + + explicit OverlayObject(const std::string & name); + virtual ~OverlayObject(); + + virtual std::string getName(); + virtual void hide(); + virtual void show(); + virtual bool isTextureReady(); + virtual void updateTextureSize(unsigned int width, unsigned int height); + virtual ScopedPixelBuffer getBuffer(); + virtual void setPosition(double left, double top); + virtual void setDimensions(double width, double height); + virtual bool isVisible(); + virtual unsigned int getTextureWidth(); + virtual unsigned int getTextureHeight(); + +protected: + const std::string name_; + Ogre::Overlay * overlay_; + Ogre::PanelOverlayElement * panel_; + Ogre::MaterialPtr panel_material_; + Ogre::TexturePtr texture_; + +private: +}; + +// Ogre::Overlay* createOverlay(std::string name); +// Ogre::PanelOverlayElement* createOverlayPanel(Ogre::Overlay* overlay); +// Ogre::MaterialPtr createOverlayMaterial(Ogre::Overlay* overlay); +} // namespace jsk_rviz_plugins + +#endif // JSK_OVERLAY_UTILS_HPP_ diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp new file mode 100644 index 0000000000..b80f06f645 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.cpp @@ -0,0 +1,251 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "mrm_summary_overlay_display.hpp" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +void insertNewlines(std::string & str, const size_t max_line_length) +{ + size_t index = max_line_length; + + while (index < str.size()) { + str.insert(index, "\n"); + index = index + max_line_length + 1; + } +} + +std::optional generateMrmMessage(diagnostic_msgs::msg::DiagnosticStatus diag_status) +{ + if (diag_status.hardware_id == "" || diag_status.hardware_id == "system_error_monitor") { + return std::nullopt; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + std::string msg = "- ERROR: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } else if (diag_status.level == diagnostic_msgs::msg::DiagnosticStatus::WARN) { + std::string msg = "- WARN: " + diag_status.name + " (" + diag_status.message + ")"; + insertNewlines(msg, 100); + return msg; + } + return std::nullopt; +} + +MrmSummaryOverlayDisplay::MrmSummaryOverlayDisplay() +{ + const Screen * screen_info = DefaultScreenOfDisplay(XOpenDisplay(NULL)); + + constexpr float hight_4k = 2160.0; + const float scale = static_cast(screen_info->height) / hight_4k; + const auto left = static_cast(std::round(1024 * scale)); + const auto top = static_cast(std::round(128 * scale)); + + property_text_color_ = new rviz_common::properties::ColorProperty( + "Text Color", QColor(25, 255, 240), "text color", this, SLOT(updateVisualization()), this); + property_left_ = new rviz_common::properties::IntProperty( + "Left", left, "Left of the plotter window", this, SLOT(updateVisualization()), this); + property_left_->setMin(0); + property_top_ = new rviz_common::properties::IntProperty( + "Top", top, "Top of the plotter window", this, SLOT(updateVisualization())); + property_top_->setMin(0); + + property_value_height_offset_ = new rviz_common::properties::IntProperty( + "Value height offset", 0, "Height offset of the plotter window", this, + SLOT(updateVisualization())); + property_font_size_ = new rviz_common::properties::IntProperty( + "Font Size", 10, "Font Size", this, SLOT(updateVisualization()), this); + property_font_size_->setMin(1); + property_max_letter_num_ = new rviz_common::properties::IntProperty( + "Max Letter Num", 100, "Max Letter Num", this, SLOT(updateVisualization()), this); + property_max_letter_num_->setMin(10); +} + +MrmSummaryOverlayDisplay::~MrmSummaryOverlayDisplay() +{ + if (initialized()) { + overlay_->hide(); + } +} + +void MrmSummaryOverlayDisplay::onInitialize() +{ + RTDClass::onInitialize(); + static int count = 0; + rviz_common::UniformStringStream ss; + ss << "MrmSummaryOverlayDisplayObject" << count++; + overlay_.reset(new jsk_rviz_plugins::OverlayObject(ss.str())); + + overlay_->show(); + + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +void MrmSummaryOverlayDisplay::onEnable() +{ + subscribe(); + overlay_->show(); +} + +void MrmSummaryOverlayDisplay::onDisable() +{ + unsubscribe(); + reset(); + overlay_->hide(); +} + +void MrmSummaryOverlayDisplay::update(float wall_dt, float ros_dt) +{ + (void)wall_dt; + (void)ros_dt; + + // MRM summary + std::vector mrm_comfortable_stop_summary_list = {}; + std::vector mrm_emergency_stop_summary_list = {}; + { + std::lock_guard message_lock(mutex_); + if (last_msg_ptr_) { + for (const auto & diag_status : last_msg_ptr_->status.diag_latent_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_comfortable_stop_summary_list.push_back(msg.value()); + } + } + for (const auto & diag_status : last_msg_ptr_->status.diag_single_point_fault) { + const std::optional msg = generateMrmMessage(diag_status); + if (msg != std::nullopt) { + mrm_emergency_stop_summary_list.push_back(msg.value()); + } + } + } + } + + // Display + QColor background_color; + background_color.setAlpha(0); + jsk_rviz_plugins::ScopedPixelBuffer buffer = overlay_->getBuffer(); + QImage hud = buffer.getQImage(*overlay_); + hud.fill(background_color); + + QPainter painter(&hud); + painter.setRenderHint(QPainter::Antialiasing, true); + + const int w = overlay_->getTextureWidth() - line_width_; + const int h = overlay_->getTextureHeight() - line_width_; + + // text + QColor text_color(property_text_color_->getColor()); + text_color.setAlpha(255); + painter.setPen(QPen(text_color, static_cast(2), Qt::SolidLine)); + QFont font = painter.font(); + font.setPixelSize(property_font_size_->getInt()); + font.setBold(true); + painter.setFont(font); + + std::ostringstream output_text; + output_text << std::fixed + << "Comfortable Stop MRM Summary: " << int(mrm_comfortable_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_comfortable_stop_summary_list) { + output_text << mrm_element << std::endl; + } + output_text << "Emergency Stop MRM Summary: " << int(mrm_emergency_stop_summary_list.size()) + << std::endl; + for (const auto & mrm_element : mrm_emergency_stop_summary_list) { + output_text << mrm_element << std::endl; + } + + // same as above, but align on right side + painter.drawText( + 0, std::min(property_value_height_offset_->getInt(), h - 1), w, + std::max(h - property_value_height_offset_->getInt(), 1), Qt::AlignLeft | Qt::AlignTop, + output_text.str().c_str()); + painter.end(); + updateVisualization(); +} + +void MrmSummaryOverlayDisplay::processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) +{ + if (!isEnabled()) { + return; + } + + { + std::lock_guard message_lock(mutex_); + last_msg_ptr_ = msg_ptr; + } + + queueRender(); +} + +void MrmSummaryOverlayDisplay::updateVisualization() +{ + const int texture_size = property_font_size_->getInt() * property_max_letter_num_->getInt(); + overlay_->updateTextureSize(texture_size, texture_size); + overlay_->setPosition(property_left_->getInt(), property_top_->getInt()); + overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight()); +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::MrmSummaryOverlayDisplay, rviz_common::Display) diff --git a/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp new file mode 100644 index 0000000000..d0b0e32c37 --- /dev/null +++ b/common/tier4_system_rviz_plugin/src/mrm_summary_overlay_display.hpp @@ -0,0 +1,108 @@ +// 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. + +// Copyright (c) 2014, JSK Lab +// All rights reserved. +// +// Software License Agreement (BSD License) +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// * Neither the name of {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +// BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +// CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +// LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +// ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE.S SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#ifndef MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ +#define MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ + +#include +#include + +#ifndef Q_MOC_RUN +#include "jsk_overlay_utils.hpp" + +#include +#include +#include +#include + +#include + +#endif + +namespace rviz_plugins +{ +class MrmSummaryOverlayDisplay +: public rviz_common::RosTopicDisplay + +{ + Q_OBJECT + +public: + MrmSummaryOverlayDisplay(); + ~MrmSummaryOverlayDisplay() override; + + void onInitialize() override; + void onEnable() override; + void onDisable() override; + +private Q_SLOTS: + void updateVisualization(); + +protected: + void update(float wall_dt, float ros_dt) override; + void processMessage( + const autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr msg_ptr) override; + jsk_rviz_plugins::OverlayObject::Ptr overlay_; + rviz_common::properties::ColorProperty * property_text_color_; + rviz_common::properties::IntProperty * property_left_; + rviz_common::properties::IntProperty * property_top_; + rviz_common::properties::IntProperty * property_value_height_offset_; + rviz_common::properties::FloatProperty * property_value_scale_; + rviz_common::properties::IntProperty * property_font_size_; + rviz_common::properties::IntProperty * property_max_letter_num_; + // QImage hud_; + +private: + static constexpr int line_width_ = 2; + static constexpr int hand_width_ = 4; + + std::mutex mutex_; + autoware_auto_system_msgs::msg::HazardStatusStamped::ConstSharedPtr last_msg_ptr_; +}; +} // namespace rviz_plugins + +#endif // MRM_SUMMARY_OVERLAY_DISPLAY_HPP_ diff --git a/common/tier4_target_object_type_rviz_plugin/package.xml b/common/tier4_target_object_type_rviz_plugin/package.xml index 6209a5aaae..04d2f28d9b 100644 --- a/common/tier4_target_object_type_rviz_plugin/package.xml +++ b/common/tier4_target_object_type_rviz_plugin/package.xml @@ -10,6 +10,7 @@ ament_cmake_auto autoware_cmake + cv_bridge libqt5-core libqt5-gui libqt5-widgets diff --git a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp index 371480eccd..8a962cea57 100644 --- a/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp +++ b/common/tier4_vehicle_rviz_plugin/src/tools/acceleration_meter.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #endif diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index 6eff270e78..e68dff6b0e 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include #include @@ -31,6 +33,8 @@ #include #endif +#include +#include namespace autoware::motion::control::autonomous_emergency_braking { using diagnostic_msgs::msg::DiagnosticStatus; diff --git a/control/control_performance_analysis/launch/controller_performance_analysis.launch.xml b/control/control_performance_analysis/launch/control_performance_analysis.launch.xml similarity index 100% rename from control/control_performance_analysis/launch/controller_performance_analysis.launch.xml rename to control/control_performance_analysis/launch/control_performance_analysis.launch.xml diff --git a/control/control_performance_analysis/schema/control_performance_analysis.schema.json b/control/control_performance_analysis/schema/control_performance_analysis.schema.json new file mode 100644 index 0000000000..f7acf3778b --- /dev/null +++ b/control/control_performance_analysis/schema/control_performance_analysis.schema.json @@ -0,0 +1,62 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Control Performance Analysis Node", + "type": "object", + "definitions": { + "control_performance_analysis": { + "type": "object", + "properties": { + "curvature_interval_length": { + "type": "number", + "default": 5.0, + "description": "Used for estimating current curvature." + }, + "prevent_zero_division_value": { + "type": "number", + "default": 0.001, + "description": "Value to avoid zero division.." + }, + "odom_interval": { + "type": "integer", + "default": 0, + "description": "Interval between odom messages, increase it for smoother curve." + }, + "acceptable_max_distance_to_waypoint": { + "type": "number", + "default": 2.0, + "description": "Maximum distance between trajectory point and vehicle [m]." + }, + "acceptable_max_yaw_difference_rad": { + "type": "number", + "default": 0.95, + "description": "Maximum yaw difference between trajectory point and vehicle [rad]." + }, + "low_pass_filter_gain": { + "type": "number", + "default": 1.0472, + "description": "Low pass filter gain." + } + }, + "required": [ + "curvature_interval_length", + "prevent_zero_division_value", + "odom_interval", + "acceptable_max_distance_to_waypoint", + "acceptable_max_yaw_difference_rad", + "low_pass_filter_gain" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/control_performance_analysis" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/control_performance_analysis/src/control_performance_analysis_node.cpp b/control/control_performance_analysis/src/control_performance_analysis_node.cpp index ad845c86fc..768aed321b 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_node.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_node.cpp @@ -47,7 +47,7 @@ ControlPerformanceAnalysisNode::ControlPerformanceAnalysisNode( // Node Parameters. param_.curvature_interval_length_ = declare_parameter("curvature_interval_length"); param_.prevent_zero_division_value_ = declare_parameter("prevent_zero_division_value"); - param_.odom_interval_ = declare_parameter("odom_interval"); + param_.odom_interval_ = declare_parameter("odom_interval"); param_.acceptable_max_distance_to_waypoint_ = declare_parameter("acceptable_max_distance_to_waypoint"); param_.acceptable_max_yaw_difference_rad_ = diff --git a/control/control_validator/src/control_validator.cpp b/control/control_validator/src/control_validator.cpp index 6c3996bfd2..ffce38f009 100644 --- a/control/control_validator/src/control_validator.cpp +++ b/control/control_validator/src/control_validator.cpp @@ -16,9 +16,6 @@ #include "control_validator/utils.hpp" -#include -#include - #include #include #include diff --git a/control/control_validator/src/debug_marker.cpp b/control/control_validator/src/debug_marker.cpp index c94c22c807..1b92aee0bb 100644 --- a/control/control_validator/src/debug_marker.cpp +++ b/control/control_validator/src/debug_marker.cpp @@ -14,8 +14,8 @@ #include "control_validator/debug_marker.hpp" -#include -#include +#include +#include #include #include diff --git a/control/control_validator/src/utils.cpp b/control/control_validator/src/utils.cpp index 1b11d1e55c..fa8261e9c3 100644 --- a/control/control_validator/src/utils.cpp +++ b/control/control_validator/src/utils.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index c146957bbd..dd05ab226f 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -18,8 +18,6 @@ #include "lane_departure_checker/lane_departure_checker.hpp" #include -#include -#include #include #include #include @@ -34,6 +32,8 @@ #include #include +#include +#include #include #include diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index f788e64316..b40a0d0473 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -14,8 +14,10 @@ #include "lane_departure_checker/lane_departure_checker_node.hpp" +#include #include #include +#include #include #include #include diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0854340ed2..186481e92c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -447,7 +447,8 @@ class MPC * @param param Trajectory filtering parameters. */ void setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param); + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics); /** * @brief Reset the previous result of MPC. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index b108e76029..441101abfc 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -187,7 +187,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase * @brief Set the current trajectory using the received message. * @param msg Received trajectory message. */ - void setTrajectory(const Trajectory & msg); + void setTrajectory(const Trajectory & msg, const Odometry & current_kinematics); /** * @brief Check if the received data is valid. diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index 89136f294a..036e6a32a9 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -99,7 +99,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist); + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment); /** * @brief linearly interpolate the given trajectory assuming a base indexing and a new desired @@ -122,14 +123,14 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj); /** * @brief recalculate the velocity field (vx) of the MPCTrajectory with dynamic smoothing - * @param [in] start_idx index of the trajectory point from which to start smoothing - * @param [in] start_vel initial velocity to set at the start_idx + * @param [in] start_seg_idx segment index of the trajectory point from which to start smoothing + * @param [in] start_vel initial velocity to set at the start_seg_idx * @param [in] acc_lim limit on the acceleration * @param [in] tau constant to control the smoothing (high-value = very smooth) * @param [inout] traj MPCTrajectory for which to calculate the smoothed velocity */ void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj); /** diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 82ebadafa1..0d7aa8aa8b 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -175,13 +175,20 @@ Float32MultiArrayStamped MPC::generateDiagData( } void MPC::setReferenceTrajectory( - const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param) + const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param, + const Odometry & current_kinematics) { + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + const double ego_offset_to_segment = motion_utils::calcLongitudinalOffsetToSegment( + trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position); + const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg); // resampling - const auto [success_resample, mpc_traj_resampled] = - MPCUtils::resampleMPCTrajectoryByDistance(mpc_traj_raw, param.traj_resample_dist); + const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance( + mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment); if (!success_resample) { warn_throttle("[setReferenceTrajectory] spline error when resampling by distance"); return; @@ -391,13 +398,13 @@ MPCTrajectory MPC::applyVelocityDynamicsFilter( return input; } - const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + const size_t nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold, ego_nearest_yaw_threshold); MPCTrajectory output = input; MPCUtils::dynamicSmoothingVelocity( - nearest_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, + nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit, m_param.velocity_time_constant, output); auto last_point = output.back(); diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 751873cacb..3d19104112 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -36,7 +36,7 @@ namespace autoware::motion::control::mpc_lateral_controller { MpcLateralController::MpcLateralController(rclcpp::Node & node) -: clock_(node.get_clock()), logger_(node.get_logger()) +: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller")) { const auto dp_int = [&](const std::string & s) { return node.declare_parameter(s); }; const auto dp_bool = [&](const std::string & s) { return node.declare_parameter(s); }; @@ -227,7 +227,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( trajectory_follower::InputData const & input_data) { // set input data - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -319,7 +319,7 @@ bool MpcLateralController::isSteerConverged(const AckermannLateralCommand & cmd) bool MpcLateralController::isReady(const trajectory_follower::InputData & input_data) { - setTrajectory(input_data.current_trajectory); + setTrajectory(input_data.current_trajectory, input_data.current_odometry); m_current_kinematic_state = input_data.current_odometry; m_current_steering = input_data.current_steering; @@ -339,7 +339,8 @@ bool MpcLateralController::isReady(const trajectory_follower::InputData & input_ return true; } -void MpcLateralController::setTrajectory(const Trajectory & msg) +void MpcLateralController::setTrajectory( + const Trajectory & msg, const Odometry & current_kinematics) { m_current_trajectory = msg; @@ -353,7 +354,7 @@ void MpcLateralController::setTrajectory(const Trajectory & msg) return; } - m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param); + m_mpc.setReferenceTrajectory(msg, m_trajectory_filtering_param, current_kinematics); // update trajectory buffer to check the trajectory shape change. m_trajectory_buffer.push_back(m_current_trajectory); diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 6d6e980675..d3fc5fba0b 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -27,6 +27,19 @@ namespace autoware::motion::control::mpc_lateral_controller { +namespace +{ +double calcLongitudinalOffset( + const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back, + const geometry_msgs::msg::Point & p_target) +{ + const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0}; + const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0}; + + return segment_vec.dot(target_vec) / segment_vec.norm(); +} +} // namespace + namespace MPCUtils { using tier4_autoware_utils::calcDistance2d; @@ -77,7 +90,8 @@ void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector resampleMPCTrajectoryByDistance( - const MPCTrajectory & input, const double resample_interval_dist) + const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx, + const double ego_offset_to_segment) { MPCTrajectory output; @@ -92,7 +106,18 @@ std::pair resampleMPCTrajectoryByDistance( } std::vector output_arclength; - for (double s = 0; s < input_arclength.back(); s += resample_interval_dist) { + // To accurately sample the ego point, resample separately in the forward direction and the + // backward direction from the current position. + for (double s = std::clamp( + input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0, + input_arclength.back() - 1e-6); + 0 <= s; s -= resample_interval_dist) { + output_arclength.push_back(s); + } + std::reverse(output_arclength.begin(), output_arclength.end()); + for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) + + resample_interval_dist; + s < input_arclength.back(); s += resample_interval_dist) { output_arclength.push_back(s); } @@ -274,13 +299,17 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj) } void dynamicSmoothingVelocity( - const size_t start_idx, const double start_vel, const double acc_lim, const double tau, + const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau, MPCTrajectory & traj) { double curr_v = start_vel; - traj.vx.at(start_idx) = start_vel; + // set current velocity in both start and end point of the segment + traj.vx.at(start_seg_idx) = start_vel; + if (1 < traj.vx.size()) { + traj.vx.at(start_seg_idx + 1) = start_vel; + } - for (size_t i = start_idx + 1; i < traj.size(); ++i) { + for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) { const double ds = calcDistance2d(traj, i, i - 1); const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits::epsilon()); const double a = tau / std::max(tau + dt, std::numeric_limits::epsilon()); @@ -320,29 +349,40 @@ bool calcNearestPoseInterp( return true; } - auto calcSquaredDist = [](const Pose & p, const MPCTrajectory & t, const size_t idx) { - const double dx = p.position.x - t.x.at(idx); - const double dy = p.position.y - t.y.at(idx); - return dx * dx + dy * dy; - }; - /* get second nearest index = next to nearest_index */ - const size_t next = static_cast( - std::min(static_cast(*nearest_index) + 1, static_cast(traj_size) - 1)); - const size_t prev = - static_cast(std::max(static_cast(*nearest_index) - 1, static_cast(0))); - const double dist_to_next = calcSquaredDist(self_pose, traj, next); - const double dist_to_prev = calcSquaredDist(self_pose, traj, prev); - const size_t second_nearest_index = (dist_to_next < dist_to_prev) ? next : prev; - - const double a_sq = calcSquaredDist(self_pose, traj, *nearest_index); - const double b_sq = calcSquaredDist(self_pose, traj, second_nearest_index); - const double dx3 = traj.x.at(*nearest_index) - traj.x.at(second_nearest_index); - const double dy3 = traj.y.at(*nearest_index) - traj.y.at(second_nearest_index); - const double c_sq = dx3 * dx3 + dy3 * dy3; + const auto [prev, next] = [&]() -> std::pair { + if (*nearest_index == 0) { + return std::make_pair(0, 1); + } + if (*nearest_index == traj_size - 1) { + return std::make_pair(traj_size - 2, traj_size - 1); + } + geometry_msgs::msg::Point nearest_traj_point; + nearest_traj_point.x = traj.x.at(*nearest_index); + nearest_traj_point.y = traj.y.at(*nearest_index); + geometry_msgs::msg::Point next_nearest_traj_point; + next_nearest_traj_point.x = traj.x.at(*nearest_index + 1); + next_nearest_traj_point.y = traj.y.at(*nearest_index + 1); + + const double signed_length = + calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position); + if (signed_length <= 0) { + return std::make_pair(*nearest_index - 1, *nearest_index); + } + return std::make_pair(*nearest_index, *nearest_index + 1); + }(); + + geometry_msgs::msg::Point next_traj_point; + next_traj_point.x = traj.x.at(next); + next_traj_point.y = traj.y.at(next); + geometry_msgs::msg::Point prev_traj_point; + prev_traj_point.x = traj.x.at(prev); + prev_traj_point.y = traj.y.at(prev); + const double traj_seg_length = + tier4_autoware_utils::calcDistance2d(prev_traj_point, next_traj_point); /* if distance between two points are too close */ - if (c_sq < 1.0E-5) { + if (traj_seg_length < 1.0E-5) { nearest_pose->position.x = traj.x.at(*nearest_index); nearest_pose->position.y = traj.y.at(*nearest_index); nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index)); @@ -351,18 +391,15 @@ bool calcNearestPoseInterp( } /* linear interpolation */ - const double alpha = std::max(std::min(0.5 * (c_sq - a_sq + b_sq) / c_sq, 1.0), 0.0); - nearest_pose->position.x = - alpha * traj.x.at(*nearest_index) + (1 - alpha) * traj.x.at(second_nearest_index); - nearest_pose->position.y = - alpha * traj.y.at(*nearest_index) + (1 - alpha) * traj.y.at(second_nearest_index); - const double tmp_yaw_err = - normalizeRadian(traj.yaw.at(*nearest_index) - traj.yaw.at(second_nearest_index)); - const double nearest_yaw = - normalizeRadian(traj.yaw.at(second_nearest_index) + alpha * tmp_yaw_err); + const double ratio = std::clamp( + calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length, + 0.0, 1.0); + nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next); + nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next); + const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next)); + const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err); nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw); - *nearest_time = alpha * traj.relative_time.at(*nearest_index) + - (1 - alpha) * traj.relative_time.at(second_nearest_index); + *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next); return true; } diff --git a/control/mpc_lateral_controller/test/test_mpc.cpp b/control/mpc_lateral_controller/test/test_mpc.cpp index 6f8a6fb598..dade035daf 100644 --- a/control/mpc_lateral_controller/test/test_mpc.cpp +++ b/control/mpc_lateral_controller/test/test_mpc.cpp @@ -163,7 +163,9 @@ class MPCTest : public ::testing::Test mpc.initializeSteeringPredictor(); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); } nav_msgs::msg::Odometry makeOdometry(const geometry_msgs::msg::Pose & pose, const double velocity) @@ -221,7 +223,9 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn) // Init parameters and reference trajectory initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; @@ -237,7 +241,8 @@ TEST_F(MPCTest, OsqpCalculate) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -262,7 +267,9 @@ TEST_F(MPCTest, OsqpCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit, steer_tau); @@ -300,7 +307,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate) // Init filters mpc.initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz); // Init trajectory - mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param); + const auto current_kinematics = makeOdometry(dummy_straight_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_straight_trajectory, trajectory_param, current_kinematics); // Calculate MPC AckermannLateralCommand ctrl_cmd; Trajectory pred_traj; @@ -315,7 +323,9 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn) { MPC mpc; initializeMPC(mpc); - mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param); + const auto current_kinematics = + makeOdometry(dummy_right_turn_trajectory.points.front().pose, 0.0); + mpc.setReferenceTrajectory(dummy_right_turn_trajectory, trajectory_param, current_kinematics); std::shared_ptr vehicle_model_ptr = std::make_shared(wheelbase, steer_limit); diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 55b672aa48..d57cb8c78c 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -38,6 +38,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) sub_control_cmd_ = node->create_subscription( "control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { control_cmd_ = *msg; }); + sub_trajectory_follower_control_cmd_ = node->create_subscription( + "trajectory_follower_control_cmd", 1, [this](const AckermannControlCommand::SharedPtr msg) { + trajectory_follower_control_cmd_ = *msg; + }); sub_kinematics_ = node->create_subscription( "kinematics", 1, [this](const Odometry::SharedPtr msg) { kinematics_ = *msg; }); @@ -261,7 +265,10 @@ bool AutonomousMode::isModeChangeAvailable() const bool speed_lower_deviation_ok = speed_deviation >= param.speed_lower_threshold; // No engagement if the vehicle is moving but the target speed is zero. - const bool stop_ok = !(std::abs(current_speed) > 0.1 && std::abs(target_control_speed) < 0.01); + const bool is_stop_cmd_indicated = + std::abs(target_control_speed) < 0.01 || + std::abs(trajectory_follower_control_cmd_.longitudinal.speed) < 0.01; + const bool stop_ok = !(std::abs(current_speed) > 0.1 && is_stop_cmd_indicated); // No engagement if the large acceleration is commanded. const bool large_acceleration_ok = !hasDangerAcceleration(); diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index de0dd8a543..e10d64e367 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -67,6 +67,7 @@ class AutonomousMode : public ModeChangeBase using Odometry = nav_msgs::msg::Odometry; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; rclcpp::Subscription::SharedPtr sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_trajectory_follower_control_cmd_; rclcpp::Subscription::SharedPtr sub_kinematics_; rclcpp::Subscription::SharedPtr sub_trajectory_; rclcpp::Logger logger_; @@ -79,6 +80,7 @@ class AutonomousMode : public ModeChangeBase EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; + AckermannControlCommand trajectory_follower_control_cmd_; Odometry kinematics_; Trajectory trajectory_; vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp index 8685d62649..9c01f7bc26 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/longitudinal_controller_utils.hpp @@ -66,11 +66,6 @@ double getPitchByPose(const Quaternion & quaternion); double getPitchByTraj( const Trajectory & trajectory, const size_t closest_idx, const double wheel_base); -/** - * @brief calculate elevation angle - */ -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to); - /** * @brief calculate vehicle pose after time delay by moving the vehicle at current velocity and * acceleration for delayed time diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 41d98a4d15..921cd3995f 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -365,7 +365,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro * @param [in] current_vel current velocity of the vehicle */ double applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel); + const Motion target_motion, const double dt, const double current_vel, const Shift & shift); /** * @brief update variables for debugging about pitch diff --git a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp index 70cdbcaf17..9791a1f0b5 100644 --- a/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/src/longitudinal_controller_utils.cpp @@ -91,41 +91,22 @@ double getPitchByTraj( return 0.0; } - for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { - const double dist = tier4_autoware_utils::calcDistance2d( - trajectory.points.at(nearest_idx), trajectory.points.at(i)); - if (dist > wheel_base) { - // calculate pitch from trajectory between rear wheel (nearest) and front center (i) - return calcElevationAngle(trajectory.points.at(nearest_idx), trajectory.points.at(i)); + const auto [prev_idx, next_idx] = [&]() { + for (size_t i = nearest_idx + 1; i < trajectory.points.size(); ++i) { + const double dist = tier4_autoware_utils::calcDistance2d( + trajectory.points.at(nearest_idx), trajectory.points.at(i)); + if (dist > wheel_base) { + // calculate pitch from trajectory between rear wheel (nearest) and front center (i) + return std::make_pair(nearest_idx, i); + } } - } - - // close to goal - for (size_t i = trajectory.points.size() - 1; i > 0; --i) { - const double dist = - tier4_autoware_utils::calcDistance2d(trajectory.points.back(), trajectory.points.at(i)); - - if (dist > wheel_base) { - // calculate pitch from trajectory - // between wheelbase behind the end of trajectory (i) and the end of trajectory (back) - return calcElevationAngle(trajectory.points.at(i), trajectory.points.back()); - } - } + // NOTE: The ego pose is close to the goal. + return std::make_pair( + std::min(nearest_idx, trajectory.points.size() - 2), trajectory.points.size() - 1); + }(); - // calculate pitch from trajectory between the beginning and end of trajectory - return calcElevationAngle(trajectory.points.at(0), trajectory.points.back()); -} - -double calcElevationAngle(const TrajectoryPoint & p_from, const TrajectoryPoint & p_to) -{ - const double dx = p_from.pose.position.x - p_to.pose.position.x; - const double dy = p_from.pose.position.y - p_to.pose.position.y; - const double dz = p_from.pose.position.z - p_to.pose.position.z; - - const double dxy = std::max(std::hypot(dx, dy), std::numeric_limits::epsilon()); - const double pitch = std::atan2(dz, dxy); - - return pitch; + return tier4_autoware_utils::calcElevationAngle( + trajectory.points.at(prev_idx).pose.position, trajectory.points.at(next_idx).pose.position); } Pose calcPoseAfterTimeDelay( diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 6d3489811b..644effce50 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -30,7 +30,7 @@ namespace autoware::motion::control::pid_longitudinal_controller PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node_parameters_(node.get_node_parameters_interface()), clock_(node.get_clock()), - logger_(node.get_logger()), + logger_(node.get_logger().get_child("longitudinal_controller")), diagnostic_updater_(&node) { using std::placeholders::_1; @@ -531,10 +531,8 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s); }; - // if current operation mode is not autonomous mode, then change state to stopped - if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) { - return changeState(ControlState::STOPPED); - } + const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; // transit state // in DRIVE state @@ -543,6 +541,12 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d return changeState(ControlState::EMERGENCY); } + if (!is_under_control && stopped_condition && keep_stopped_condition) { + // NOTE: When the ego is stopped on manual driving, since the driving state may transit to + // autonomous, keep_stopped_condition should be checked. + return changeState(ControlState::STOPPED); + } + if (m_enable_smooth_stop) { if (stopping_condition) { // predictions after input time delay @@ -609,8 +613,14 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d // in EMERGENCY state if (m_control_state == ControlState::EMERGENCY) { - if (stopped_condition && !emergency_condition) { - return changeState(ControlState::STOPPED); + if (!emergency_condition) { + if (stopped_condition) { + return changeState(ControlState::STOPPED); + } + if (!is_under_control) { + // NOTE: On manual driving, no need stopping to exit the emergency. + return changeState(ControlState::DRIVE); + } } return; } @@ -644,7 +654,8 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( m_debug_values.setValues(DebugValues::TYPE::PREDICTED_VEL, pred_vel_in_target); raw_ctrl_cmd.vel = target_motion.vel; - raw_ctrl_cmd.acc = applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target); + raw_ctrl_cmd.acc = + applyVelocityFeedback(target_motion, control_data.dt, pred_vel_in_target, control_data.shift); RCLCPP_DEBUG( logger_, "[feedback control] vel: %3.3f, acc: %3.3f, dt: %3.3f, v_curr: %3.3f, v_ref: %3.3f " @@ -817,7 +828,7 @@ double PidLongitudinalController::applySlopeCompensation( const double pitch_limited = std::min(std::max(pitch, m_min_pitch_rad), m_max_pitch_rad); // Acceleration command is always positive independent of direction (= shift) when car is running - double sign = (shift == Shift::Forward) ? -1 : (shift == Shift::Reverse ? 1 : 0); + double sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0); double compensated_acc = input_acc + sign * 9.81 * std::sin(pitch_limited); return compensated_acc; } @@ -919,14 +930,16 @@ double PidLongitudinalController::predictedVelocityInTargetPoint( } double PidLongitudinalController::applyVelocityFeedback( - const Motion target_motion, const double dt, const double current_vel) + const Motion target_motion, const double dt, const double current_vel, const Shift & shift) { - const double current_vel_abs = std::fabs(current_vel); - const double target_vel_abs = std::fabs(target_motion.vel); - const bool is_under_control = m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + // NOTE: Acceleration command is always positive even if the ego drives backward. + const double vel_sign = (shift == Shift::Forward) ? 1.0 : (shift == Shift::Reverse ? -1.0 : 0.0); + const double diff_vel = (target_motion.vel - current_vel) * vel_sign; + const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && + m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; const bool enable_integration = - (current_vel_abs > m_current_vel_threshold_pid_integrate) && is_under_control; - const double error_vel_filtered = m_lpf_vel_error->filter(target_vel_abs - current_vel_abs); + (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); const double pid_acc = @@ -939,8 +952,8 @@ double PidLongitudinalController::applyVelocityFeedback( // deviation will be bigger. constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety - const double ff_scale = - std::clamp(current_vel_abs / std::max(target_vel_abs, 0.1), ff_scale_min, ff_scale_max); + const double ff_scale = std::clamp( + std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); const double ff_acc = target_motion.acc * ff_scale; const double feedback_acc = ff_acc + pid_acc; diff --git a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp index d9fc404ce6..5c7698180f 100644 --- a/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp +++ b/control/pid_longitudinal_controller/test/test_longitudinal_controller_utils.cpp @@ -158,33 +158,6 @@ TEST(TestLongitudinalControllerUtils, getPitchByTraj) std::atan2(0.5, 1)); } -TEST(TestLongitudinalControllerUtils, calcElevationAngle) -{ - using autoware_auto_planning_msgs::msg::TrajectoryPoint; - TrajectoryPoint p_from; - p_from.pose.position.x = 0.0; - p_from.pose.position.y = 0.0; - TrajectoryPoint p_to; - p_to.pose.position.x = 1.0; - p_to.pose.position.y = 0.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), 0.0); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_4); - p_to.pose.position.x = 0.0; - p_to.pose.position.z = 1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), -M_PI_2); - p_to.pose.position.x = 1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); - p_to.pose.position.x = -1.0; - p_to.pose.position.z = -1.0; - EXPECT_DOUBLE_EQ(longitudinal_utils::calcElevationAngle(p_from, p_to), M_PI_4); -} - TEST(TestLongitudinalControllerUtils, calcPoseAfterTimeDelay) { using geometry_msgs::msg::Pose; diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index 91b110b35b..f0c49b07e6 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp @@ -57,7 +57,10 @@ enum TYPE { namespace pure_pursuit { PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node) -: clock_(node.get_clock()), logger_(node.get_logger()), tf_buffer_(clock_), tf_listener_(tf_buffer_) +: clock_(node.get_clock()), + logger_(node.get_logger().get_child("lateral_controller")), + tf_buffer_(clock_), + tf_listener_(tf_buffer_) { pure_pursuit_ = std::make_unique(); diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 92844c61f6..191e894622 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -15,6 +15,8 @@ nominal: vel_lim: 25.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [5.0, 4.0] lon_jerk_lim: [5.0, 4.0] lat_acc_lim: [5.0, 4.0] @@ -23,6 +25,8 @@ on_transition: vel_lim: 50.0 reference_speed_points: [20.0, 30.0] + steer_lim: [1.0, 0.8] + steer_rate_lim: [1.0, 0.8] lon_acc_lim: [1.0, 0.9] lon_jerk_lim: [0.5, 0.4] lat_acc_lim: [2.0, 1.8] diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp index 6a33855fb4..4431534a76 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.cpp @@ -29,7 +29,8 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & const auto s = p.reference_speed_points.size(); if ( p.lon_acc_lim.size() != s || p.lon_jerk_lim.size() != s || p.lat_acc_lim.size() != s || - p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s) { + p.lat_jerk_lim.size() != s || p.actual_steer_diff_lim.size() != s || p.steer_lim.size() != s || + p.steer_rate_lim.size() != s) { std::cerr << "VehicleCmdFilter::setParam() There is a size mismatch in the parameter. " "Parameter initialization failed." << std::endl; @@ -39,7 +40,18 @@ bool VehicleCmdFilter::setParameterWithValidation(const VehicleCmdFilterParam & param_ = p; return true; } - +void VehicleCmdFilter::setSteerLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_lim = v; + setParameterWithValidation(tmp); +} +void VehicleCmdFilter::setSteerRateLim(LimitArray v) +{ + auto tmp = param_; + tmp.steer_rate_lim = v; + setParameterWithValidation(tmp); +} void VehicleCmdFilter::setLonAccLim(LimitArray v) { auto tmp = param_; @@ -151,12 +163,36 @@ void VehicleCmdFilter::limitActualSteerDiff( void VehicleCmdFilter::limitLateralSteer(AckermannControlCommand & input) const { - // TODO(Horibe): parametrize the max steering angle. - // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration - // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. - constexpr float steer_limit = M_PI_2; + const float steer_limit = getSteerLim(); + input.lateral.steering_tire_angle = std::clamp(input.lateral.steering_tire_angle, -steer_limit, steer_limit); + + // TODO(Horibe): support steering greater than PI/2. Now the lateral acceleration + // calculation does not support bigger steering value than PI/2 due to tan/atan calculation. + if (std::abs(input.lateral.steering_tire_angle) > M_PI_2f) { + std::cerr << "[vehicle_Cmd_gate] limitLateralSteer(): steering limit is set to pi/2 since the " + "current filtering logic can not handle the steering larger than pi/2. Please " + "check the steering angle limit." + << std::endl; + + std::clamp(input.lateral.steering_tire_angle, -M_PI_2f, M_PI_2f); + } +} + +void VehicleCmdFilter::limitLateralSteerRate(const double dt, AckermannControlCommand & input) const +{ + const float steer_rate_limit = getSteerRateLim(); + + // for steering angle rate + input.lateral.steering_tire_rotation_rate = + std::clamp(input.lateral.steering_tire_rotation_rate, -steer_rate_limit, steer_rate_limit); + + // for steering angle + const float steer_diff_limit = steer_rate_limit * dt; + float ds = input.lateral.steering_tire_angle - prev_cmd_.lateral.steering_tire_angle; + ds = std::clamp(ds, -steer_diff_limit, steer_diff_limit); + input.lateral.steering_tire_angle = prev_cmd_.lateral.steering_tire_angle + ds; } void VehicleCmdFilter::filterAll( @@ -165,6 +201,7 @@ void VehicleCmdFilter::filterAll( { const auto cmd_orig = cmd; limitLateralSteer(cmd); + limitLateralSteerRate(dt, cmd); limitLongitudinalWithJerk(dt, cmd); limitLongitudinalWithAcc(dt, cmd); limitLongitudinalWithVel(cmd); @@ -267,6 +304,14 @@ double VehicleCmdFilter::getLatJerkLim() const { return interpolateFromSpeed(param_.lat_jerk_lim); } +double VehicleCmdFilter::getSteerLim() const +{ + return interpolateFromSpeed(param_.steer_lim); +} +double VehicleCmdFilter::getSteerRateLim() const +{ + return interpolateFromSpeed(param_.steer_rate_lim); +} double VehicleCmdFilter::getSteerDiffLim() const { return interpolateFromSpeed(param_.actual_steer_diff_lim); diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp index a79b8a38ba..cf7a1f627a 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_filter.hpp @@ -37,6 +37,8 @@ struct VehicleCmdFilterParam LimitArray lon_jerk_lim; LimitArray lat_acc_lim; LimitArray lat_jerk_lim; + LimitArray steer_lim; + LimitArray steer_rate_lim; LimitArray actual_steer_diff_lim; }; class VehicleCmdFilter @@ -47,6 +49,8 @@ class VehicleCmdFilter void setWheelBase(double v) { param_.wheel_base = v; } void setVelLim(double v) { param_.vel_lim = v; } + void setSteerLim(LimitArray v); + void setSteerRateLim(LimitArray v); void setLonAccLim(LimitArray v); void setLonJerkLim(LimitArray v); void setLatAccLim(LimitArray v); @@ -64,6 +68,7 @@ class VehicleCmdFilter void limitActualSteerDiff( const double current_steer_angle, AckermannControlCommand & input) const; void limitLateralSteer(AckermannControlCommand & input) const; + void limitLateralSteerRate(const double dt, AckermannControlCommand & input) const; void filterAll( const double dt, const double current_steer_angle, AckermannControlCommand & input, IsFilterActivated & is_activated) const; @@ -90,6 +95,8 @@ class VehicleCmdFilter double getLonJerkLim() const; double getLatAccLim() const; double getLatJerkLim() const; + double getSteerLim() const; + double getSteerRateLim() const; double getSteerDiffLim() const; }; } // namespace vehicle_cmd_gate diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 6d3893cbb9..2f2083f626 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -169,6 +169,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("nominal.vel_lim"); p.reference_speed_points = declare_parameter>("nominal.reference_speed_points"); + p.steer_lim = declare_parameter>("nominal.steer_lim"); + p.steer_rate_lim = declare_parameter>("nominal.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("nominal.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("nominal.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("nominal.lat_acc_lim"); @@ -184,6 +186,8 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) p.vel_lim = declare_parameter("on_transition.vel_lim"); p.reference_speed_points = declare_parameter>("on_transition.reference_speed_points"); + p.steer_lim = declare_parameter>("on_transition.steer_lim"); + p.steer_rate_lim = declare_parameter>("on_transition.steer_rate_lim"); p.lon_acc_lim = declare_parameter>("on_transition.lon_acc_lim"); p.lon_jerk_lim = declare_parameter>("on_transition.lon_jerk_lim"); p.lat_acc_lim = declare_parameter>("on_transition.lat_acc_lim"); diff --git a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp index 1156656385..f02235ed1e 100644 --- a/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp +++ b/control/vehicle_cmd_gate/test/src/test_filter_in_vehicle_cmd_gate_node.cpp @@ -45,6 +45,8 @@ void print_values(int i, const T1 & name, const T2 & a, const T3 &... b) // global params const std::vector reference_speed_points = {5., 10., 15., 20.}; +const std::vector steer_lim = {0.5, 0.3, 0.2, 0.1}; +const std::vector steer_rate_lim = {0.5, 0.3, 0.2, 0.1}; const std::vector lon_acc_lim = {1.5, 1.0, 0.8, 0.6}; const std::vector lon_jerk_lim = {1.4, 0.9, 0.7, 0.5}; const std::vector lat_acc_lim = {2.0, 1.6, 1.2, 0.8}; @@ -336,6 +338,8 @@ std::shared_ptr generateNode() node_options.append_parameter_override("wheel_base", wheelbase); override("nominal.reference_speed_points", reference_speed_points); override("nominal.reference_speed_points", reference_speed_points); + override("nominal.steer_lim", steer_lim); + override("nominal.steer_rate_lim", steer_rate_lim); override("nominal.lon_acc_lim", lon_acc_lim); override("nominal.lon_jerk_lim", lon_jerk_lim); override("nominal.lat_acc_lim", lat_acc_lim); diff --git a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp index 1791493aeb..b8200490dd 100644 --- a/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp +++ b/control/vehicle_cmd_gate/test/src/test_vehicle_cmd_filter.cpp @@ -31,12 +31,15 @@ constexpr double NOMINAL_INTERVAL = 1.0; void setFilterParams( vehicle_cmd_gate::VehicleCmdFilter & f, double v, LimitArray speed_points, LimitArray a, - LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, const double wheelbase) + LimitArray j, LimitArray lat_a, LimitArray lat_j, LimitArray steer_diff, LimitArray steer_lim, + LimitArray steer_rate_lim, const double wheelbase) { vehicle_cmd_gate::VehicleCmdFilterParam p; p.vel_lim = v; p.wheel_base = wheelbase; p.reference_speed_points = speed_points; + p.steer_lim = steer_lim; + p.steer_rate_lim = steer_rate_lim; p.lat_acc_lim = lat_a; p.lat_jerk_lim = lat_j; p.lon_acc_lim = a; @@ -97,8 +100,8 @@ double calcLatJerk( void test_1d_limit( double ego_v, double V_LIM, double A_LIM, double J_LIM, double LAT_A_LIM, double LAT_J_LIM, - double STEER_DIFF, const AckermannControlCommand & prev_cmd, - const AckermannControlCommand & raw_cmd) + double STEER_DIFF, double STEER_LIM, double STEER_RATE_LIM, + const AckermannControlCommand & prev_cmd, const AckermannControlCommand & raw_cmd) { const double WHEELBASE = 3.0; const double DT = 0.1; // [s] @@ -106,7 +109,8 @@ void test_1d_limit( vehicle_cmd_gate::VehicleCmdFilter filter; filter.setCurrentSpeed(ego_v); setFilterParams( - filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, WHEELBASE); + filter, V_LIM, {0.0}, {A_LIM}, {J_LIM}, {LAT_A_LIM}, {LAT_J_LIM}, {STEER_DIFF}, {STEER_LIM}, + {STEER_RATE_LIM}, WHEELBASE); filter.setPrevCmd(prev_cmd); // velocity filter @@ -233,6 +237,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) const std::vector lat_a_arr = {0.01, 1.0, 100.0}; const std::vector lat_j_arr = {0.01, 1.0, 100.0}; const std::vector steer_diff_arr = {0.01, 1.0, 100.0}; + const std::vector steer_lim_arr = {0.01, 1.0, 100.0}; + const std::vector steer_rate_lim_arr = {0.01, 1.0, 100.0}; const std::vector ego_v_arr = {0.0, 0.1, 1.0, 3.0, 15.0}; const std::vector prev_cmd_arr = { @@ -249,8 +255,13 @@ TEST(VehicleCmdFilter, VehicleCmdFilter) for (const auto & prev_cmd : prev_cmd_arr) { for (const auto & raw_cmd : raw_cmd_arr) { for (const auto & steer_diff : steer_diff_arr) { - for (const auto & ego_v : ego_v_arr) { - test_1d_limit(ego_v, v, a, j, la, lj, steer_diff, prev_cmd, raw_cmd); + for (const auto & steer : steer_lim_arr) { + for (const auto & steer_rate : steer_rate_lim_arr) { + for (const auto & ego_v : ego_v_arr) { + test_1d_limit( + ego_v, v, a, j, la, lj, steer_diff, steer, steer_rate, prev_cmd, raw_cmd); + } + } } } } @@ -271,6 +282,8 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) p.wheel_base = WHEELBASE; p.vel_lim = 20.0; p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + p.steer_lim = std::vector{0.1, 0.2, 0.3}; + p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; p.lon_acc_lim = std::vector{0.3, 0.4, 0.5}; p.lon_jerk_lim = std::vector{0.4, 0.4, 0.7}; p.lat_acc_lim = std::vector{0.1, 0.2, 0.3}; @@ -293,7 +306,16 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) const auto set_speed_and_reset_prev = [&](const auto & current_vel) { filter.setCurrentSpeed(current_vel); }; - + const auto _limitSteer = [&](const auto & in) { + auto out = in; + filter.limitLateralSteer(out); + return out; + }; + const auto _limitSteerRate = [&](const auto & in) { + auto out = in; + filter.limitLateralSteerRate(DT, out); + return out; + }; const auto _limitLongitudinalWithVel = [&](const auto & in) { auto out = in; filter.limitLongitudinalWithVel(out); @@ -334,6 +356,77 @@ TEST(VehicleCmdFilter, VehicleCmdFilterInterpolate) EXPECT_NEAR(_limitLongitudinalWithVel(orig_cmd).longitudinal.speed, 20.0, ep); } + // steer angle lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_lim = std::vector{0.1, 0.2, 0.3}; + { + set_speed_and_reset_prev(0.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(2.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.1, ep); + + set_speed_and_reset_prev(3.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.15, ep); + + set_speed_and_reset_prev(5.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.2 + 0.1 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + + set_speed_and_reset_prev(15.0); + EXPECT_NEAR(_limitSteer(orig_cmd).lateral.steering_tire_angle, 0.3, ep); + } + + // steer angle rate lim + // p.reference_speed_points = std::vector{2.0, 4.0, 10.0}; + // p.steer_rate_lim = std::vector{0.2, 0.1, 0.05}; + { + const auto calcSteerRateFromAngle = [&](const auto & cmd) { + return (cmd.steering_tire_angle - 0.0) / DT; + }; + autoware_auto_control_msgs::msg::AckermannLateralCommand filtered; + + set_speed_and_reset_prev(0.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(2.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.2, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.2, ep); + + set_speed_and_reset_prev(3.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.15, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.15, ep); + + set_speed_and_reset_prev(5.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 1.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 1.0 / 6.0, ep); + + set_speed_and_reset_prev(8.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.1 - 0.05 * 4.0 / 6.0, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.1 - 0.05 * 4.0 / 6.0, ep); + + set_speed_and_reset_prev(10.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + + set_speed_and_reset_prev(15.0); + filtered = _limitSteerRate(orig_cmd).lateral; + EXPECT_NEAR(calcSteerRateFromAngle(filtered), 0.05, ep); + EXPECT_NEAR(filtered.steering_tire_rotation_rate, 0.05, ep); + } + // longitudinal acc lim { set_speed_and_reset_prev(0.0); diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 116055dd9e..375819f8e7 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -239,6 +239,7 @@ def launch_setup(context, *args, **kwargs): ("steering", "/vehicle/status/steering_status"), ("trajectory", "/planning/scenario_planning/trajectory"), ("control_cmd", "/control/command/control_cmd"), + ("trajectory_follower_control_cmd", "/control/trajectory_follower/control_cmd"), ("control_mode_report", "/vehicle/status/control_mode"), ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), # output diff --git a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml index 3b32d080ad..a767d2d253 100644 --- a/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_twist_estimator/ar_tag_based_localizer.launch.xml @@ -6,6 +6,7 @@ + @@ -13,9 +14,9 @@ - + - + diff --git a/launch/tier4_localization_launch/package.xml b/launch/tier4_localization_launch/package.xml index f00752d1c1..6b3b3d3bbb 100644 --- a/launch/tier4_localization_launch/package.xml +++ b/launch/tier4_localization_launch/package.xml @@ -21,6 +21,7 @@ eagleye_rt ekf_localizer gyro_odometer + landmark_tf_caster ndt_scan_matcher pointcloud_preprocessor pose_initializer 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 eab2a56607..178e193dc2 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 @@ -1,9 +1,9 @@ - + - + @@ -11,15 +11,14 @@ - - + - + @@ -59,15 +58,16 @@ --> - + - + + @@ -75,19 +75,18 @@ - - + + - - - - + + + - + @@ -120,7 +119,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 463340efde..df833296e3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -1,25 +1,23 @@ - + - + - + - - - + @@ -46,8 +44,9 @@ - + + - + @@ -78,15 +77,16 @@ - + - + + @@ -94,13 +94,12 @@ - - + + - - - - + + + @@ -151,7 +150,7 @@ - + @@ -184,7 +183,7 @@ - + @@ -220,7 +219,7 @@ - + @@ -292,7 +291,7 @@ - + @@ -301,7 +300,7 @@ - + @@ -383,7 +382,7 @@ - + @@ -391,8 +390,18 @@ - + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ca5d1d0f7e..77a3e345ec 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,16 +1,16 @@ + - + - - + @@ -29,14 +29,15 @@ - + + + + + - - - - + @@ -64,11 +65,16 @@ + + + + + - + @@ -94,10 +100,12 @@ + + - + @@ -106,11 +114,14 @@ + + + - + @@ -119,14 +130,16 @@ + - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 5b6134dc45..23e0297dc5 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -4,13 +4,11 @@ - - - + @@ -19,29 +17,27 @@ - + - + + - - - + - - + @@ -74,7 +70,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index a47e6a86f1..cee6829ea3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -1,32 +1,33 @@ - + - - + - + + - + + - + - + @@ -42,7 +43,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index a5d8d2113d..93d395ca3e 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -35,7 +35,6 @@ def __init__(self, context): ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] - self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] self.downsize_ratio_z_axis = self.pointcloud_map_filter_param["downsize_ratio_z_axis"] @@ -46,47 +45,40 @@ def __init__(self, context): ] self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] self.publish_debug_pcd = self.pointcloud_map_filter_param["publish_debug_pcd"] + self.use_pointcloud_map = LaunchConfiguration("use_pointcloud_map").perform(context) def create_pipeline(self): - if self.use_down_sample_filter: - return self.create_down_sample_pipeline() + if self.use_pointcloud_map == "true": + return self.create_compare_map_pipeline() else: - return self.create_normal_pipeline() + return self.create_no_compare_map_pipeline() - def create_normal_pipeline(self): + def create_no_compare_map_pipeline(self): components = [] components.append( ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name="voxel_based_compare_map_filter", + package="pointcloud_preprocessor", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", + name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), - ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), ], parameters=[ { - "distance_threshold": self.distance_threshold, - "downsize_ratio_z_axis": self.downsize_ratio_z_axis, - "timer_interval_ms": self.timer_interval_ms, - "use_dynamic_map_loading": self.use_dynamic_map_loading, - "map_update_distance_threshold": self.map_update_distance_threshold, - "map_loader_radius": self.map_loader_radius, - "publish_debug_pcd": self.publish_debug_pcd, - "input_frame": "map", + "voxel_size_x": self.voxel_size, + "voxel_size_y": self.voxel_size, + "voxel_size_z": self.voxel_size, } ], extra_arguments=[ - {"use_intra_process_comms": False}, + {"use_intra_process_comms": LaunchConfiguration("use_intra_process")} ], - ) + ), ) return components - def create_down_sample_pipeline(self): + def create_compare_map_pipeline(self): components = [] down_sample_topic = ( "/perception/obstacle_segmentation/pointcloud_map_filtered/downsampled/pointcloud" @@ -94,7 +86,7 @@ def create_down_sample_pipeline(self): components.append( ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", + plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", name="voxel_grid_downsample_filter", remappings=[ ("input", LaunchConfiguration("input_topic")), @@ -177,6 +169,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") + add_launch_arg("use_pointcloud_map", "true") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 8e2e23b510..52c8aedff8 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -2,14 +2,14 @@ - - - - - + + + + + - - + + 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 7ed5860612..12190fb659 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 @@ -122,12 +122,16 @@ def create_ransac_pipeline(self): plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", namespace="plane_fitting", - remappings=[("output", "concatenated/pointcloud")], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", "concatenated/pointcloud"), + ], parameters=[ { "input_topics": self.ground_segmentation_param["ransac_input_topics"], "output_frame": LaunchConfiguration("base_frame"), "timeout_sec": 1.0, + "input_twist_topic_type": "odom", } ], extra_arguments=[ @@ -432,11 +436,15 @@ def get_additional_lidars_concatenated_component(input_topics, output_topic): package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], @@ -448,11 +456,15 @@ def get_single_frame_obstacle_segmentation_concatenated_component(input_topics, package="pointcloud_preprocessor", plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="concatenate_no_ground_data", - remappings=[("output", output_topic)], + remappings=[ + ("~/input/odom", "/localization/kinematic_state"), + ("output", output_topic), + ], parameters=[ { "input_topics": input_topics, "output_frame": LaunchConfiguration("base_frame"), + "input_twist_topic_type": "odom", } ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index c41177e511..36d43bab74 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,10 +1,8 @@ - - @@ -13,6 +11,7 @@ + @@ -24,11 +23,11 @@ - + - + @@ -59,8 +58,9 @@ - + + - + + @@ -88,15 +89,15 @@ description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6" /> - + - + - + @@ -109,7 +110,7 @@ - + @@ -127,10 +128,10 @@ - + - + @@ -156,29 +157,30 @@ - - + - + + - + - + + @@ -194,7 +196,7 @@ - + diff --git a/localization/ar_tag_based_localizer/README.md b/localization/ar_tag_based_localizer/README.md deleted file mode 100644 index c7b970ebbc..0000000000 --- a/localization/ar_tag_based_localizer/README.md +++ /dev/null @@ -1,173 +0,0 @@ -# AR Tag Based Localizer - -**ArTagBasedLocalizer** is a vision-based localization package. - - - -This package uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. -The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. - -This package includes two nodes. - -- `ar_tag_based_localizer` : A node that detects AR-Tags from camera images and publishes the pose of the ego vehicle. -- `tag_tf_caster` : A node that publishes the pose of the AR-Tags applied in Lanelet2 as `tf_static`. - -## Inputs / Outputs - -### `ar_tag_based_localizer` node - -#### Input - -| Name | Type | Description | -| :-------------------- | :----------------------------- | :----------- | -| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | -| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | - -#### Output - -| Name | Type | Description | -| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | -| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | -| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | -| `tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | - -### `tag_tf_caster` node - -#### Input - -| Name | Type | Description | -| :--------------------- | :------------------------------------------- | :--------------- | -| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | - -#### Output - -| Name | Type | Description | -| :---------- | :------------------------------------- | :----------------- | -| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | - -## How to launch - -When launching Autoware, specify `artag` for `pose_source`. - -```bash -ros2 launch autoware_launch ... \ - pose_source:=artag \ - ... -``` - -### Rosbag - -#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) - -This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). -Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. - -It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. - -![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) - -#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) - -Please remap the topic names and play it. - -```bash -ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ - --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ - /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info -``` - -This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. - -The image below shows the trajectory when the sample is executed and plotted. - -![sample_result](./doc_image/sample_result.png) - -The pull request video below should also be helpful. - - - -## Architecture - -![node diagram](./doc_image/node_diagram.drawio.svg) - -## Principle - -### `ar_tag_based_localizer` node - -![principle](./doc_image/principle.png) - -### `tag_tf_caster` node - -The definitions of the tags written to the map are introduced in the next section. See `Map Specifications`. - -The `tag_tf_caster` node publishes the TF from the map to the tag. - -- Translation : The center of the four vertices of the tag -- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. - -Users can define tags as Lanelet2 4-vertex polygons. -In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the tag in that case is difficult to calculate. -So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the tag will not publish tf_static. - -## Map specifications - -For this package to function correctly, the pose of the AR-Tags must be specified in the Lanelet2 map format that Autoware can interpret. - -The four vertices of AR-Tag are defined counterclockwise. - -The order of the four vertices is defined as follows. In the coordinate system of AR-Tag, - -- the x-axis is parallel to the vector from the first vertex to the second vertex -- the y-axis is parallel to the vector from the second vertex to the third vertex - -![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) - -### example of `lanelet2_map.osm` - -The values provided below are placeholders. -Ensure to input the correct coordinates corresponding to the actual location where the AR-Tag is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. - -```xml -... - - - - - - - - - - - - - - - - - - - - - - - - - - -... - - - - - - - - - - - - -... - -``` diff --git a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml deleted file mode 100644 index d0eef44229..0000000000 --- a/localization/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ /dev/null @@ -1,24 +0,0 @@ -/**: - ros__parameters: - # marker_size - marker_size: 0.6 - - # target_tag_ids - target_tag_ids: ['0','1','2','3','4','5','6'] - - # base_covariance - # This value is dynamically scaled according to the distance at which AR tags are detected. - base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] - - # Detect AR-Tags within this range and publish the pose of ego vehicle - distance_threshold: 13.0 # [m] - - # Detector parameters - # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 - detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] - min_marker_size: 0.02 diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp b/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp deleted file mode 100644 index 06401dad8f..0000000000 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/utils.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#ifndef AR_TAG_BASED_LOCALIZER__UTILS_HPP_ -#define AR_TAG_BASED_LOCALIZER__UTILS_HPP_ - -#include - -#include -#include - -/** - * @brief ros_camera_info_to_aruco_cam_params gets the camera intrinsics from a CameraInfo message - * and copies them to aruco's data structure - * @param cam_info - * @param use_rectified_parameters if true, the intrinsics are taken from cam_info.P and the - * distortion parameters are set to 0. Otherwise, cam_info.K and cam_info.D are taken. - * @return - */ -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters); - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); - -#endif // AR_TAG_BASED_LOCALIZER__UTILS_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml b/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml deleted file mode 100644 index 6a9c3dcd17..0000000000 --- a/localization/ar_tag_based_localizer/launch/tag_tf_caster.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/localization/ar_tag_based_localizer/src/utils.cpp b/localization/ar_tag_based_localizer/src/utils.cpp deleted file mode 100644 index 9f58283028..0000000000 --- a/localization/ar_tag_based_localizer/src/utils.cpp +++ /dev/null @@ -1,116 +0,0 @@ -// 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. - -// This source code is derived from the https://github.com/pal-robotics/aruco_ros. -// Here is the license statement. -/***************************** - Copyright 2011 Rafael Muñoz Salinas. All rights reserved. - - Redistribution and use in source and binary forms, with or without modification, are - permitted provided that the following conditions are met: - - 1. Redistributions of source code must retain the above copyright notice, this list of - conditions and the following disclaimer. - - 2. Redistributions in binary form must reproduce the above copyright notice, this list - of conditions and the following disclaimer in the documentation and/or other materials - provided with the distribution. - - THIS SOFTWARE IS PROVIDED BY Rafael Muñoz Salinas ''AS IS'' AND ANY EXPRESS OR IMPLIED - WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND - FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Rafael Muñoz Salinas OR - CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING - NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF - ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - - The views and conclusions contained in the software and documentation are those of the - authors and should not be interpreted as representing official policies, either expressed - or implied, of Rafael Muñoz Salinas. - ********************************/ - -#include "ar_tag_based_localizer/utils.hpp" - -#include -#include - -aruco::CameraParameters ros_camera_info_to_aruco_cam_params( - const sensor_msgs::msg::CameraInfo & cam_info, bool use_rectified_parameters) -{ - cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - cv::Mat distortion_coeff(4, 1, CV_64FC1); - cv::Size size(static_cast(cam_info.width), static_cast(cam_info.height)); - - if (use_rectified_parameters) { - camera_matrix.setTo(0); - camera_matrix.at(0, 0) = cam_info.p[0]; - camera_matrix.at(0, 1) = cam_info.p[1]; - camera_matrix.at(0, 2) = cam_info.p[2]; - camera_matrix.at(0, 3) = cam_info.p[3]; - camera_matrix.at(1, 0) = cam_info.p[4]; - camera_matrix.at(1, 1) = cam_info.p[5]; - camera_matrix.at(1, 2) = cam_info.p[6]; - camera_matrix.at(1, 3) = cam_info.p[7]; - camera_matrix.at(2, 0) = cam_info.p[8]; - camera_matrix.at(2, 1) = cam_info.p[9]; - camera_matrix.at(2, 2) = cam_info.p[10]; - camera_matrix.at(2, 3) = cam_info.p[11]; - - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } else { - cv::Mat camera_matrix_from_k(3, 3, CV_64FC1, 0.0); - for (int i = 0; i < 9; ++i) { - camera_matrix_from_k.at(i % 3, i - (i % 3) * 3) = cam_info.k[i]; - } - camera_matrix_from_k.copyTo(camera_matrix(cv::Rect(0, 0, 3, 3))); - - if (cam_info.d.size() == 4) { - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = cam_info.d[i]; - } - } else { - RCLCPP_WARN( - rclcpp::get_logger("ar_tag_based_localizer"), - "length of camera_info D vector is not 4, assuming zero distortion..."); - for (int i = 0; i < 4; ++i) { - distortion_coeff.at(i, 0) = 0; - } - } - } - - return {camera_matrix, distortion_coeff, size}; -} - -tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker) -{ - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); - - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); - - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); - - return tf2::Transform(tf_rot, tf_orig); -} diff --git a/localization/ekf_localizer/CMakeLists.txt b/localization/ekf_localizer/CMakeLists.txt index d5cd85aa2e..05c91bff13 100644 --- a/localization/ekf_localizer/CMakeLists.txt +++ b/localization/ekf_localizer/CMakeLists.txt @@ -16,6 +16,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(ekf_localizer_lib SHARED src/ekf_localizer.cpp src/covariance.cpp + src/diagnostics.cpp src/mahalanobis.cpp src/measurement.cpp src/state_transition.cpp diff --git a/localization/ekf_localizer/README.md b/localization/ekf_localizer/README.md index 748b5ee5be..6492f20331 100644 --- a/localization/ekf_localizer/README.md +++ b/localization/ekf_localizer/README.md @@ -88,6 +88,10 @@ The parameters and input topic names can be set in the `ekf_localizer.launch` fi The estimated twist with covariance. +- diagnostics (diagnostic_msgs/DiagnosticArray) + + The diagnostic information. + ### Published TF - base_link @@ -148,6 +152,21 @@ The parameters are set in `launch/ekf_localizer.launch` . note: process noise for positions x & y are calculated automatically from nonlinear dynamics. +### For diagnostics + +| Name | Type | Description | Default value | +| :------------------------------------ | :----- | :----------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| pose_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 50 | +| pose_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Pose Topic update not happening continuously for a certain number of times. | 250 | +| twist_no_update_count_threshold_warn | size_t | The threshold at which a WARN state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 50 | +| twist_no_update_count_threshold_error | size_t | The threshold at which an ERROR state is triggered due to the Twist Topic update not happening continuously for a certain number of times. | 250 | + +### Misc + +| Name | Type | Description | Default value | +| :-------------------------------- | :----- | :------------------------------------------------------------------------------------------------- | :------------- | +| threshold_observable_velocity_mps | double | Minimum value for velocity that will be used for EKF. Mainly used for dead zone in velocity sensor | 0.0 (disabled) | + ## How to tune EKF parameters ### 0. Preliminaries @@ -194,6 +213,23 @@ Note that, although the dimension gets larger since the analytical expansion can

+## Diagnostics + +

+ +

+ +### The conditions that result in a WARN state + +- The node is not in the activate state. +- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_warn`/`twist_no_update_count_threshold_warn`. +- The timestamp of the Pose/Twist topic is beyond the delay compensation range. +- The Pose/Twist topic is beyond the range of Mahalanobis distance for covariance estimation. + +### The conditions that result in an ERROR state + +- The number of consecutive no measurement update via the Pose/Twist topic exceeds the `pose_no_update_count_threshold_error`/`twist_no_update_count_threshold_error`. + ## Known issues - In the presence of multiple inputs with yaw estimation, yaw bias `b_k` in the current EKF state would not make any sense, since it is intended to capture the extrinsic parameter's calibration error of a sensor. Thus, future work includes introducing yaw bias for each sensor with yaw estimation. diff --git a/localization/ekf_localizer/config/ekf_localizer.param.yaml b/localization/ekf_localizer/config/ekf_localizer.param.yaml index 4d3f5b9643..667217d259 100644 --- a/localization/ekf_localizer/config/ekf_localizer.param.yaml +++ b/localization/ekf_localizer/config/ekf_localizer.param.yaml @@ -21,3 +21,12 @@ proc_stddev_yaw_c: 0.005 proc_stddev_vx_c: 10.0 proc_stddev_wz_c: 5.0 + + # for diagnostics + pose_no_update_count_threshold_warn: 50 + pose_no_update_count_threshold_error: 250 + twist_no_update_count_threshold_warn: 50 + twist_no_update_count_threshold_error: 250 + + # for velocity measurement limitation (Set 0.0 if you want to ignore) + threshold_observable_velocity_mps: 0.0 # [m/s] diff --git a/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp new file mode 100644 index 0000000000..f4dc6436f6 --- /dev/null +++ b/localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp @@ -0,0 +1,40 @@ +// 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__DIAGNOSTICS_HPP_ +#define EKF_LOCALIZER__DIAGNOSTICS_HPP_ + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated); + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( + const std::string & measurement_type, const size_t no_update_count, + const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( + const std::string & measurement_type, const size_t queue_size); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( + const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, + const double delay_time_threshold); +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( + const std::string & measurement_type, const bool is_passed_mahalanobis_gate, + const double mahalanobis_distance, const double mahalanobis_distance_threshold); + +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array); + +#endif // EKF_LOCALIZER__DIAGNOSTICS_HPP_ diff --git a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp index a4ae47b670..4fc2305cc7 100644 --- a/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -126,6 +127,8 @@ class EKFLocalizer : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_biased_pose_; //!< @brief ekf estimated yaw bias publisher rclcpp::Publisher::SharedPtr pub_biased_pose_cov_; + //!< @brief diagnostics publisher + rclcpp::Publisher::SharedPtr pub_diag_; //!< @brief initial pose subscriber rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief measurement pose with covariance subscriber @@ -144,6 +147,7 @@ class EKFLocalizer : public rclcpp::Node rclcpp::TimerBase::SharedPtr timer_tf_; //!< @brief tf broadcaster std::shared_ptr tf_br_; + //!< @brief extended kalman filter instance. TimeDelayKalmanFilter ekf_; Simple1DFilter z_filter_; @@ -167,6 +171,22 @@ class EKFLocalizer : public rclcpp::Node bool is_activated_; + size_t pose_no_update_count_; + size_t pose_queue_size_; + bool pose_is_passed_delay_gate_; + double pose_delay_time_; + double pose_delay_time_threshold_; + bool pose_is_passed_mahalanobis_gate_; + double pose_mahalanobis_distance_; + + size_t twist_no_update_count_; + size_t twist_queue_size_; + bool twist_is_passed_delay_gate_; + double twist_delay_time_; + double twist_delay_time_threshold_; + bool twist_is_passed_mahalanobis_gate_; + double twist_mahalanobis_distance_; + AgedObjectQueue pose_queue_; AgedObjectQueue twist_queue_; @@ -221,13 +241,13 @@ class EKFLocalizer : public rclcpp::Node * @brief compute EKF update with pose measurement * @param pose measurement value */ - void measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + bool measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); /** * @brief compute EKF update with pose measurement * @param twist measurement value */ - void measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); + bool measurementUpdateTwist(const geometry_msgs::msg::TwistWithCovarianceStamped & twist); /** * @brief get transform from frame_id @@ -246,6 +266,11 @@ class EKFLocalizer : public rclcpp::Node */ void publishEstimateResult(); + /** + * @brief publish diagnostics message + */ + void publishDiagnostics(); + /** * @brief for debug */ diff --git a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp index c74fa9be79..01ef658cf4 100644 --- a/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp +++ b/localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp @@ -39,7 +39,17 @@ class HyperParameters twist_smoothing_steps(node->declare_parameter("twist_smoothing_steps", 2)), proc_stddev_vx_c(node->declare_parameter("proc_stddev_vx_c", 5.0)), proc_stddev_wz_c(node->declare_parameter("proc_stddev_wz_c", 1.0)), - proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)) + proc_stddev_yaw_c(node->declare_parameter("proc_stddev_yaw_c", 0.005)), + pose_no_update_count_threshold_warn( + node->declare_parameter("pose_no_update_count_threshold_warn", 50)), + pose_no_update_count_threshold_error( + node->declare_parameter("pose_no_update_count_threshold_error", 250)), + twist_no_update_count_threshold_warn( + node->declare_parameter("twist_no_update_count_threshold_warn", 50)), + twist_no_update_count_threshold_error( + node->declare_parameter("twist_no_update_count_threshold_error", 250)), + threshold_observable_velocity_mps( + node->declare_parameter("threshold_observable_velocity_mps", 0.5)) { } @@ -59,6 +69,11 @@ class HyperParameters const double proc_stddev_vx_c; //!< @brief vx process noise const double proc_stddev_wz_c; //!< @brief wz process noise const double proc_stddev_yaw_c; //!< @brief yaw process noise + const size_t pose_no_update_count_threshold_warn; + const size_t pose_no_update_count_threshold_error; + const size_t twist_no_update_count_threshold_warn; + const size_t twist_no_update_count_threshold_error; + const double threshold_observable_velocity_mps; }; #endif // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_ diff --git a/localization/ekf_localizer/media/ekf_diagnostics.png b/localization/ekf_localizer/media/ekf_diagnostics.png new file mode 100644 index 0000000000..2580d6d973 Binary files /dev/null and b/localization/ekf_localizer/media/ekf_diagnostics.png differ diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index 5bc9c5e427..005bddd3eb 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -18,6 +18,7 @@ eigen + diagnostic_msgs fmt geometry_msgs kalman_filter diff --git a/localization/ekf_localizer/src/diagnostics.cpp b/localization/ekf_localizer/src/diagnostics.cpp new file mode 100644 index 0000000000..9ff36561ab --- /dev/null +++ b/localization/ekf_localizer/src/diagnostics.cpp @@ -0,0 +1,169 @@ +// 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/diagnostics.hpp" + +#include + +#include +#include + +diagnostic_msgs::msg::DiagnosticStatus checkProcessActivated(const bool is_activated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "is_activated"; + key_value.value = is_activated ? "True" : "False"; + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_activated) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]process is not activated"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementUpdated( + const std::string & measurement_type, const size_t no_update_count, + const size_t no_update_count_threshold_warn, const size_t no_update_count_threshold_error) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_no_update_count"; + key_value.value = std::to_string(no_update_count); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_no_update_count_threshold_warn"; + key_value.value = std::to_string(no_update_count_threshold_warn); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_no_update_count_threshold_error"; + key_value.value = std::to_string(no_update_count_threshold_error); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (no_update_count >= no_update_count_threshold_warn) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]" + measurement_type + " is not updated"; + } + if (no_update_count >= no_update_count_threshold_error) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat.message = "[ERROR]" + measurement_type + " is not updated"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementQueueSize( + const std::string & measurement_type, const size_t queue_size) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_queue_size"; + key_value.value = std::to_string(queue_size); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementDelayGate( + const std::string & measurement_type, const bool is_passed_delay_gate, const double delay_time, + const double delay_time_threshold) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_is_passed_delay_gate"; + key_value.value = is_passed_delay_gate ? "True" : "False"; + stat.values.push_back(key_value); + key_value.key = measurement_type + "_delay_time"; + key_value.value = std::to_string(delay_time); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_delay_time_threshold"; + key_value.value = std::to_string(delay_time_threshold); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_passed_delay_gate) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]" + measurement_type + " topic is delay"; + } + + return stat; +} + +diagnostic_msgs::msg::DiagnosticStatus checkMeasurementMahalanobisGate( + const std::string & measurement_type, const bool is_passed_mahalanobis_gate, + const double mahalanobis_distance, const double mahalanobis_distance_threshold) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = measurement_type + "_is_passed_mahalanobis_gate"; + key_value.value = is_passed_mahalanobis_gate ? "True" : "False"; + stat.values.push_back(key_value); + key_value.key = measurement_type + "_mahalanobis_distance"; + key_value.value = std::to_string(mahalanobis_distance); + stat.values.push_back(key_value); + key_value.key = measurement_type + "_mahalanobis_distance_threshold"; + key_value.value = std::to_string(mahalanobis_distance_threshold); + stat.values.push_back(key_value); + + stat.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat.message = "OK"; + if (!is_passed_mahalanobis_gate) { + stat.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat.message = "[WARN]mahalanobis distance of " + measurement_type + " topic is large"; + } + + return stat; +} + +// The highest level within the stat_array will be reflected in the merged_stat. +// When all stat_array entries are 'OK,' the message of merged_stat will be "OK" +diagnostic_msgs::msg::DiagnosticStatus mergeDiagnosticStatus( + const std::vector & stat_array) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + + for (const auto & stat : stat_array) { + if ((stat.level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { + if (!merged_stat.message.empty()) { + merged_stat.message += "; "; + } + merged_stat.message += stat.message; + } + if (stat.level > merged_stat.level) { + merged_stat.level = stat.level; + } + for (const auto & value : stat.values) { + merged_stat.values.push_back(value); + } + } + + if (merged_stat.level == diagnostic_msgs::msg::DiagnosticStatus::OK) { + merged_stat.message = "OK"; + } + + return merged_stat; +} diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index 0e46a26add..b39112b1d8 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -15,6 +15,7 @@ #include "ekf_localizer/ekf_localizer.hpp" #include "ekf_localizer/covariance.hpp" +#include "ekf_localizer/diagnostics.hpp" #include "ekf_localizer/mahalanobis.hpp" #include "ekf_localizer/matrix_types.hpp" #include "ekf_localizer/measurement.hpp" @@ -87,6 +88,7 @@ EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOpti pub_biased_pose_ = create_publisher("ekf_biased_pose", 1); pub_biased_pose_cov_ = create_publisher( "ekf_biased_pose_with_covariance", 1); + pub_diag_ = this->create_publisher("/diagnostics", 10); sub_initialpose_ = create_subscription( "initialpose", 1, std::bind(&EKFLocalizer::callbackInitialPose, this, _1)); sub_pose_with_cov_ = create_subscription( @@ -143,6 +145,7 @@ void EKFLocalizer::timerCallback() if (!is_activated_) { warning_.warnThrottle( "The node is not activated. Provide initial pose to pose_initializer", 2000); + publishDiagnostics(); return; } @@ -176,6 +179,16 @@ void EKFLocalizer::timerCallback() DEBUG_INFO(get_logger(), "------------------------- end prediction -------------------------\n"); /* pose measurement update */ + + pose_queue_size_ = pose_queue_.size(); + pose_is_passed_delay_gate_ = true; + pose_delay_time_ = 0.0; + pose_delay_time_threshold_ = 0.0; + pose_is_passed_mahalanobis_gate_ = true; + pose_mahalanobis_distance_ = 0.0; + + bool pose_is_updated = false; + if (!pose_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Pose -------------------------"); stop_watch_.tic(); @@ -184,13 +197,27 @@ void EKFLocalizer::timerCallback() const size_t n = pose_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto pose = pose_queue_.pop_increment_age(); - measurementUpdatePose(*pose); + bool is_updated = measurementUpdatePose(*pose); + if (is_updated) { + pose_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdatePose calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Pose -------------------------\n"); } + pose_no_update_count_ = pose_is_updated ? 0 : (pose_no_update_count_ + 1); /* twist measurement update */ + + twist_queue_size_ = twist_queue_.size(); + twist_is_passed_delay_gate_ = true; + twist_delay_time_ = 0.0; + twist_delay_time_threshold_ = 0.0; + twist_is_passed_mahalanobis_gate_ = true; + twist_mahalanobis_distance_ = 0.0; + + bool twist_is_updated = false; + if (!twist_queue_.empty()) { DEBUG_INFO(get_logger(), "------------------------- start Twist -------------------------"); stop_watch_.tic(); @@ -199,11 +226,15 @@ void EKFLocalizer::timerCallback() const size_t n = twist_queue_.size(); for (size_t i = 0; i < n; ++i) { const auto twist = twist_queue_.pop_increment_age(); - measurementUpdateTwist(*twist); + bool is_updated = measurementUpdateTwist(*twist); + if (is_updated) { + twist_is_updated = true; + } } DEBUG_INFO(get_logger(), "[EKF] measurementUpdateTwist calc time = %f [ms]", stop_watch_.toc()); DEBUG_INFO(get_logger(), "------------------------- end Twist -------------------------\n"); } + twist_no_update_count_ = twist_is_updated ? 0 : (twist_no_update_count_ + 1); const double x = ekf_.getXelement(IDX::X); const double y = ekf_.getXelement(IDX::Y); @@ -235,6 +266,7 @@ void EKFLocalizer::timerCallback() /* publish ekf result */ publishEstimateResult(); + publishDiagnostics(); } void EKFLocalizer::showCurrentX() @@ -353,6 +385,11 @@ void EKFLocalizer::callbackPoseWithCovariance( void EKFLocalizer::callbackTwistWithCovariance( geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { + // Ignore twist if velocity is too small. + // Note that this inequality must not include "equal". + if (std::abs(msg->twist.twist.linear.x) < params_.threshold_observable_velocity_mps) { + msg->twist.covariance[0 * 6 + 0] = 10000.0; + } twist_queue_.push(msg); } @@ -376,7 +413,7 @@ void EKFLocalizer::initEKF() /* * measurementUpdatePose */ -void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +bool EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) { if (pose.header.frame_id != params_.pose_frame_id) { warning_.warnThrottle( @@ -400,10 +437,14 @@ 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_); + + pose_delay_time_ = std::max(delay_time, pose_delay_time_); + pose_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + pose_is_passed_delay_gate_ = false; warning_.warnThrottle( poseDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -420,7 +461,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message."); - return; + return false; } /* Gate */ @@ -431,10 +472,12 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd P_y = P_curr.block(0, 0, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + pose_mahalanobis_distance_ = std::max(distance, pose_mahalanobis_distance_); if (distance > params_.pose_gate_dist) { + pose_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.pose_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -460,12 +503,14 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* * measurementUpdateTwist */ -void EKFLocalizer::measurementUpdateTwist( +bool EKFLocalizer::measurementUpdateTwist( const geometry_msgs::msg::TwistWithCovarianceStamped & twist) { if (twist.header.frame_id != "base_link") { @@ -488,10 +533,14 @@ void EKFLocalizer::measurementUpdateTwist( delay_time = std::max(delay_time, 0.0); int delay_step = std::roundf(delay_time / ekf_dt_); + + twist_delay_time_ = std::max(delay_time, twist_delay_time_); + twist_delay_time_threshold_ = params_.extend_state_step * ekf_dt_; if (delay_step >= params_.extend_state_step) { + twist_is_passed_delay_gate_ = false; warning_.warnThrottle( twistDelayStepWarningMessage(delay_time, params_.extend_state_step, ekf_dt_), 2000); - return; + return false; } DEBUG_INFO(get_logger(), "delay_time: %f [s]", delay_time); @@ -502,7 +551,7 @@ void EKFLocalizer::measurementUpdateTwist( if (hasNan(y) || hasInf(y)) { warning_.warn( "[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message."); - return; + return false; } const Eigen::Vector2d y_ekf( @@ -512,10 +561,12 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd P_y = P_curr.block(4, 4, dim_y, dim_y); const double distance = mahalanobis(y_ekf, y, P_y); + twist_mahalanobis_distance_ = std::max(distance, twist_mahalanobis_distance_); if (distance > params_.twist_gate_dist) { + twist_is_passed_mahalanobis_gate_ = false; warning_.warnThrottle(mahalanobisWarningMessage(distance, params_.twist_gate_dist), 2000); warning_.warnThrottle("Ignore the measurement data.", 2000); - return; + return false; } DEBUG_PRINT_MAT(y.transpose()); @@ -532,6 +583,8 @@ void EKFLocalizer::measurementUpdateTwist( const Eigen::MatrixXd X_result = ekf_.getLatestX(); DEBUG_PRINT_MAT(X_result.transpose()); DEBUG_PRINT_MAT((X_result - X_curr).transpose()); + + return true; } /* @@ -607,6 +660,45 @@ void EKFLocalizer::publishEstimateResult() pub_debug_->publish(msg); } +void EKFLocalizer::publishDiagnostics() +{ + std::vector diag_status_array; + + diag_status_array.push_back(checkProcessActivated(is_activated_)); + + if (is_activated_) { + diag_status_array.push_back(checkMeasurementUpdated( + "pose", pose_no_update_count_, params_.pose_no_update_count_threshold_warn, + params_.pose_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("pose", pose_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "pose", pose_is_passed_delay_gate_, pose_delay_time_, pose_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "pose", pose_is_passed_mahalanobis_gate_, pose_mahalanobis_distance_, + params_.pose_gate_dist)); + + diag_status_array.push_back(checkMeasurementUpdated( + "twist", twist_no_update_count_, params_.twist_no_update_count_threshold_warn, + params_.twist_no_update_count_threshold_error)); + diag_status_array.push_back(checkMeasurementQueueSize("twist", twist_queue_size_)); + diag_status_array.push_back(checkMeasurementDelayGate( + "twist", twist_is_passed_delay_gate_, twist_delay_time_, twist_delay_time_threshold_)); + diag_status_array.push_back(checkMeasurementMahalanobisGate( + "twist", twist_is_passed_mahalanobis_gate_, twist_mahalanobis_distance_, + params_.twist_gate_dist)); + } + + diagnostic_msgs::msg::DiagnosticStatus diag_merged_status; + diag_merged_status = mergeDiagnosticStatus(diag_status_array); + diag_merged_status.name = "localization: " + std::string(this->get_name()); + diag_merged_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_merged_status); + pub_diag_->publish(diag_msg); +} + void EKFLocalizer::updateSimple1DFilters( const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step) { diff --git a/localization/ekf_localizer/test/test_diagnostics.cpp b/localization/ekf_localizer/test/test_diagnostics.cpp new file mode 100644 index 0000000000..f506dca1cb --- /dev/null +++ b/localization/ekf_localizer/test/test_diagnostics.cpp @@ -0,0 +1,192 @@ +// 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/diagnostics.hpp" + +#include + +TEST(TestEkfDiagnostics, CheckProcessActivated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + bool is_activated = true; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_activated = false; + stat = checkProcessActivated(is_activated); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, checkMeasurementUpdated) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const size_t no_update_count_threshold_warn = 50; + const size_t no_update_count_threshold_error = 250; + + size_t no_update_count = 0; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 1; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 49; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + no_update_count = 50; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 249; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + + no_update_count = 250; + stat = checkMeasurementUpdated( + measurement_type, no_update_count, no_update_count_threshold_warn, + no_update_count_threshold_error); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST(TestEkfDiagnostics, CheckMeasurementQueueSize) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + + size_t queue_size = 0; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + queue_size = 1; // not effect for stat.level + stat = checkMeasurementQueueSize(measurement_type, queue_size); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST(TestEkfDiagnostics, CheckMeasurementDelayGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double delay_time = 0.1; // not effect for stat.level + const double delay_time_threshold = 1.0; // not effect for stat.level + + bool is_passed_delay_gate = true; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_delay_gate = false; + stat = checkMeasurementDelayGate( + measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestEkfDiagnostics, CheckMeasurementMahalanobisGate) +{ + diagnostic_msgs::msg::DiagnosticStatus stat; + + const std::string measurement_type = "pose"; // not effect for stat.level + const double mahalanobis_distance = 0.1; // not effect for stat.level + const double mahalanobis_distance_threshold = 1.0; // not effect for stat.level + + bool is_passed_mahalanobis_gate = true; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + + is_passed_mahalanobis_gate = false; + stat = checkMeasurementMahalanobisGate( + measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance, + mahalanobis_distance_threshold); + EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST(TestLocalizationErrorMonitorDiagnostics, MergeDiagnosticStatus) +{ + diagnostic_msgs::msg::DiagnosticStatus merged_stat; + std::vector stat_array(2); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(merged_stat.message, "OK"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(1).message = "OK"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(1).message = "WARN1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN); + EXPECT_EQ(merged_stat.message, "WARN0; WARN1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK; + stat_array.at(0).message = "OK"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + stat_array.at(0).message = "WARN0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "WARN0; ERROR1"); + + stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(0).message = "ERROR0"; + stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + stat_array.at(1).message = "ERROR1"; + merged_stat = mergeDiagnosticStatus(stat_array); + EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1"); +} diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md new file mode 100644 index 0000000000..a8ebdc0c8f --- /dev/null +++ b/localization/landmark_based_localizer/README.md @@ -0,0 +1,123 @@ +# Landmark Based Localizer + +This directory contains packages for landmark-based localization. + +Landmarks are, for example + +- AR tags detected by camera +- Boards characterized by intensity detected by LiDAR + +etc. + +Since these landmarks are easy to detect and estimate pose, the ego pose can be calculated from the pose of the detected landmark if the pose of the landmark is written on the map in advance. + +Currently, landmarks are assumed to be flat. + +The following figure shows the principle of localization in the case of `ar_tag_based_localizer`. + +![principle](./doc_image/principle.png) + +This calculated ego pose is passed to the EKF, where it is fused with the twist information and used to estimate a more accurate ego pose. + +## Node diagram + +![node diagram](./doc_image/node_diagram.drawio.svg) + +### `landmark_tf_caster` node + +The definitions of the landmarks written to the map are introduced in the next section. See `Map Specifications`. + +The `landmark_tf_caster` node publishes the TF from the map to the landmark. + +- Translation : The center of the four vertices of the landmark +- Rotation : Let the vertex numbers be 1, 2, 3, 4 counterclockwise as shown in the next section. Direction is defined as the cross product of the vector from 1 to 2 and the vector from 2 to 3. + +Users can define landmarks as Lanelet2 4-vertex polygons. +In this case, it is possible to define an arrangement in which the four vertices cannot be considered to be on the same plane. The direction of the landmark in that case is difficult to calculate. +So, if the 4 vertices are considered as forming a tetrahedron and its volume exceeds the `volume_threshold` parameter, the landmark will not publish tf_static. + +### Landmark based localizer packages + +- ar_tag_based_localizer +- etc. + +## Inputs / Outputs + +### `landmark_tf_caster` node + +#### Input + +| Name | Type | Description | +| :--------------------- | :------------------------------------------- | :--------------- | +| `~/input/lanelet2_map` | `autoware_auto_mapping_msgs::msg::HADMapBin` | Data of lanelet2 | + +#### Output + +| Name | Type | Description | +| :---------- | :------------------------------------- | :----------------- | +| `tf_static` | `geometry_msgs::msg::TransformStamped` | TF from map to tag | + +## Map specifications + +For this package to work correctly, the poses of the landmarks must be specified in the Lanelet2 map format that `map_loader` and `landmark_tf_caster` can interpret. + +The four vertices of a landmark are defined counterclockwise. + +The order of the four vertices is defined as follows. In the coordinate system of a landmark, + +- the x-axis is parallel to the vector from the first vertex to the second vertex +- the y-axis is parallel to the vector from the second vertex to the third vertex + +![lanelet2 data structure](./doc_image/lanelet2_data_structure.drawio.svg) + +### Example of `lanelet2_map.osm` + +The values provided below are placeholders. +Ensure to input the correct coordinates corresponding to the actual location where the landmark is placed, such as `lat`, `lon`, `mgrs_code`, `local_x`, `local_y`. + +For example, when using the AR tag, it would look like this. + +```xml +... + + + + + + + + + + + + + + + + + + + + + + + + + + +... + + + + + + + + + + + + +... + +``` diff --git a/localization/ar_tag_based_localizer/CMakeLists.txt b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt similarity index 74% rename from localization/ar_tag_based_localizer/CMakeLists.txt rename to localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt index 89337b9bb9..15aecd1f8d 100644 --- a/localization/ar_tag_based_localizer/CMakeLists.txt +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/CMakeLists.txt @@ -14,13 +14,9 @@ ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) -# -# ar_tag_based_localizer -# ament_auto_add_executable(ar_tag_based_localizer src/ar_tag_based_localizer_node.cpp src/ar_tag_based_localizer_core.cpp - src/utils.cpp ) target_include_directories(ar_tag_based_localizer SYSTEM PUBLIC @@ -28,18 +24,6 @@ target_include_directories(ar_tag_based_localizer ) target_link_libraries(ar_tag_based_localizer ${OpenCV_LIBRARIES}) -# -# tag_tf_caster -# -ament_auto_add_executable(tag_tf_caster - src/tag_tf_caster_node.cpp - src/tag_tf_caster_core.cpp -) -target_include_directories(tag_tf_caster - SYSTEM PUBLIC - ${OpenCV_INCLUDE_DIRS} -) - ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/README.md b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md new file mode 100644 index 0000000000..b212711f38 --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/README.md @@ -0,0 +1,74 @@ +# AR Tag Based Localizer + +**ArTagBasedLocalizer** is a vision-based localization node. + + + +This node uses [the ArUco library](https://index.ros.org/p/aruco/) to detect AR-Tags from camera images and calculates and publishes the pose of the ego vehicle based on these detections. +The positions and orientations of the AR-Tags are assumed to be written in the Lanelet2 format. + +## Inputs / Outputs + +### `ar_tag_based_localizer` node + +#### Input + +| Name | Type | Description | +| :-------------------- | :---------------------------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | +| `~/input/image` | `sensor_msgs::msg::Image` | Camera Image | +| `~/input/camera_info` | `sensor_msgs::msg::CameraInfo` | Camera Info | +| `~/input/ekf_pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | EKF Pose without IMU correction. It is used to validate detected AR tags by filtering out False Positives. Only if the EKF Pose and the AR tag-detected Pose are within a certain temporal and spatial range, the AR tag-detected Pose is considered valid and published. | + +#### Output + +| Name | Type | Description | +| :------------------------------ | :---------------------------------------------- | :---------------------------------------------------------------------------------------- | +| `~/output/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | Estimated Pose | +| `~/debug/result` | `sensor_msgs::msg::Image` | [debug topic] Image in which marker detection results are superimposed on the input image | +| `/tf` | `geometry_msgs::msg::TransformStamped` | [debug topic] TF from camera to detected tag | +| `/diagnostics` | `diagnostic_msgs::msg::DiagnosticArray` | Diagnostics outputs | + +## How to launch + +When launching Autoware, set `artag` for `pose_source`. + +```bash +ros2 launch autoware_launch ... \ + pose_source:=artag \ + ... +``` + +### Rosbag + +#### [Sample rosbag and map (AWSIM data)](https://drive.google.com/file/d/1uMVwQQFcfs8JOqfoA1FqfH_fLPwQ71jK/view) + +This data is simulated data created by [AWSIM](https://tier4.github.io/AWSIM/). +Essentially, AR tag-based self-localization is not intended for such public road driving, but for driving in a smaller area, so the max driving speed is set at 15 km/h. + +It is a known problem that the timing of when each AR tag begins to be detected can cause significant changes in estimation. + +![sample_result_in_awsim](./doc_image/sample_result_in_awsim.png) + +#### [Sample rosbag and map (Real world data)](https://drive.google.com/file/d/1wiCQjyjRnYbb0dg8G6mRecdSGh8tv3zR/view) + +Please remap the topic names and play it. + +```bash +ros2 bag play /path/to/ar_tag_based_localizer_sample_bag/ -r 0.5 -s sqlite3 \ + --remap /sensing/camera/front/image:=/sensing/camera/traffic_light/image_raw \ + /sensing/camera/front/image/info:=/sensing/camera/traffic_light/camera_info +``` + +This dataset contains issues such as missing IMU data, and overall the accuracy is low. Even when running AR tag-based self-localization, significant difference from the true trajectory can be observed. + +The image below shows the trajectory when the sample is executed and plotted. + +![sample_result](./doc_image/sample_result.png) + +The pull request video below should also be helpful. + + + +## Principle + +![principle](../doc_image/principle.png) diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml new file mode 100644 index 0000000000..29260e27cd --- /dev/null +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -0,0 +1,37 @@ +/**: + ros__parameters: + # marker_size + marker_size: 0.6 + + # target_tag_ids + target_tag_ids: ['0','1','2','3','4','5','6'] + + # base_covariance + # This value is dynamically scaled according to the distance at which AR tags are detected. + base_covariance: [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.02] + + # Detect AR-Tags within this range and publish the pose of ego vehicle + distance_threshold: 13.0 # [m] + + # Detector parameters + # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 + detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] + min_marker_size: 0.02 + + # Parameters for comparison with EKF Pose + # If the difference between the EKF pose and the current pose is within the range of values set below, the current pose is published. + # [How to determine the value] + # * ekf_time_tolerance: Since it is abnormal if the data comes too old from EKF, the tentative tolerance value is set at 5 seconds. + # This value is assumed to be unaffected even if it is increased or decreased by some amount. + # * ekf_position_tolerance: Since it is possible that multiple AR tags with the same ID could be placed, the tolerance should be as small as possible. + # And if the vehicle is running only on odometry in a section without AR tags, + # it is possible that self-position estimation could be off by a few meters. + # it should be fixed by AR tag detection, so tolerance should not be smaller than 10 meters. + # Therefore, the tolerance is set at 10 meters. + ekf_time_tolerance: 5.0 # [s] + ekf_position_tolerance: 10.0 # [m] diff --git a/localization/ar_tag_based_localizer/doc_image/ar_tag_image.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/ar_tag_image.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/ar_tag_image.png diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/sample_result.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result.png diff --git a/localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png b/localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png rename to localization/landmark_based_localizer/ar_tag_based_localizer/doc_image/sample_result_in_awsim.png diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp similarity index 88% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp index b6a513b981..37725dd06c 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/include/ar_tag_based_localizer/ar_tag_based_localizer_core.hpp @@ -48,6 +48,7 @@ #include #include +#include #include #include @@ -69,14 +70,18 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void image_callback(const sensor_msgs::msg::Image::ConstSharedPtr & msg); void cam_info_callback(const sensor_msgs::msg::CameraInfo & msg); + void ekf_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); void publish_pose_as_base_link( const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id); + static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); // Parameters float marker_size_{}; std::vector target_tag_ids_; std::vector base_covariance_; double distance_threshold_squared_{}; + double ekf_time_tolerance_{}; + double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; @@ -89,15 +94,18 @@ class ArTagBasedLocalizer : public rclcpp::Node // Subscribers rclcpp::Subscription::SharedPtr image_sub_; rclcpp::Subscription::SharedPtr cam_info_sub_; + rclcpp::Subscription::SharedPtr ekf_pose_sub_; // Publishers image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr diag_pub_; // Others aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; + geometry_msgs::msg::PoseWithCovarianceStamped latest_ekf_pose_{}; }; #endif // AR_TAG_BASED_LOCALIZER__AR_TAG_BASED_LOCALIZER_CORE_HPP_ diff --git a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml similarity index 88% rename from localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml index 940f6ed53d..f6c10050b1 100644 --- a/localization/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/launch/ar_tag_based_localizer.launch.xml @@ -5,6 +5,7 @@ + @@ -12,6 +13,7 @@ + diff --git a/localization/ar_tag_based_localizer/package.xml b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml similarity index 92% rename from localization/ar_tag_based_localizer/package.xml rename to localization/landmark_based_localizer/ar_tag_based_localizer/package.xml index 4527d2ad9b..2c35be181d 100644 --- a/localization/ar_tag_based_localizer/package.xml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/package.xml @@ -12,11 +12,10 @@ autoware_cmake aruco - autoware_auto_mapping_msgs cv_bridge + diagnostic_msgs geometry_msgs image_transport - lanelet2_extension rclcpp sensor_msgs tf2_eigen diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp similarity index 68% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp index c97e117194..afa82ab3e0 100644 --- a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_core.cpp @@ -44,12 +44,12 @@ #include "ar_tag_based_localizer/ar_tag_based_localizer_core.hpp" -#include "ar_tag_based_localizer/utils.hpp" - #include #include +#include #include +#include #ifdef ROS_DISTRO_GALACTIC #include @@ -71,6 +71,8 @@ bool ArTagBasedLocalizer::setup() target_tag_ids_ = this->declare_parameter>("target_tag_ids"); base_covariance_ = this->declare_parameter>("base_covariance"); distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); + ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); float min_marker_size = static_cast(this->declare_parameter("min_marker_size")); if (detection_mode == "DM_NORMAL") { @@ -116,6 +118,9 @@ bool ArTagBasedLocalizer::setup() cam_info_sub_ = this->create_subscription( "~/input/camera_info", qos_sub, std::bind(&ArTagBasedLocalizer::cam_info_callback, this, std::placeholders::_1)); + ekf_pose_sub_ = this->create_subscription( + "~/input/ekf_pose", qos_sub, + std::bind(&ArTagBasedLocalizer::ekf_pose_callback, this, std::placeholders::_1)); /* Publishers @@ -124,6 +129,8 @@ bool ArTagBasedLocalizer::setup() image_pub_ = it_->advertise("~/debug/result", 1); pose_pub_ = this->create_publisher( "~/output/pose_with_covariance", qos_pub); + diag_pub_ = + this->create_publisher("/diagnostics", qos_pub); RCLCPP_INFO(this->get_logger(), "Setup of ar_tag_based_localizer node is successful!"); return true; @@ -194,6 +201,32 @@ void ArTagBasedLocalizer::image_callback(const sensor_msgs::msg::Image::ConstSha out_msg.image = in_image; image_pub_.publish(out_msg.toImageMsg()); } + + const int detected_tags = markers.size(); + + diagnostic_msgs::msg::DiagnosticStatus diag_status; + + if (detected_tags > 0) { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + diag_status.message = "AR tags detected. The number of tags: " + std::to_string(detected_tags); + } else { + diag_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + diag_status.message = "No AR tags detected."; + } + + diag_status.name = "localization: " + std::string(this->get_name()); + diag_status.hardware_id = this->get_name(); + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "Number of Detected AR Tags"; + key_value.value = std::to_string(detected_tags); + diag_status.values.push_back(key_value); + + diagnostic_msgs::msg::DiagnosticArray diag_msg; + diag_msg.header.stamp = this->now(); + diag_msg.status.push_back(diag_status); + + diag_pub_->publish(diag_msg); } // wait for one camera info, then shut down that subscriber @@ -203,10 +236,39 @@ void ArTagBasedLocalizer::cam_info_callback(const sensor_msgs::msg::CameraInfo & return; } - cam_param_ = ros_camera_info_to_aruco_cam_params(msg, true); + cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); + cv::Mat distortion_coeff(4, 1, CV_64FC1); + cv::Size size(static_cast(msg.width), static_cast(msg.height)); + + camera_matrix.setTo(0); + camera_matrix.at(0, 0) = msg.p[0]; + camera_matrix.at(0, 1) = msg.p[1]; + camera_matrix.at(0, 2) = msg.p[2]; + camera_matrix.at(0, 3) = msg.p[3]; + camera_matrix.at(1, 0) = msg.p[4]; + camera_matrix.at(1, 1) = msg.p[5]; + camera_matrix.at(1, 2) = msg.p[6]; + camera_matrix.at(1, 3) = msg.p[7]; + camera_matrix.at(2, 0) = msg.p[8]; + camera_matrix.at(2, 1) = msg.p[9]; + camera_matrix.at(2, 2) = msg.p[10]; + camera_matrix.at(2, 3) = msg.p[11]; + + for (int i = 0; i < 4; ++i) { + distortion_coeff.at(i, 0) = 0; + } + + cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); + cam_info_received_ = true; } +void ArTagBasedLocalizer::ekf_pose_callback( + const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + latest_ekf_pose_ = msg; +} + void ArTagBasedLocalizer::publish_pose_as_base_link( const geometry_msgs::msg::PoseStamped & msg, const std::string & camera_frame_id) { @@ -268,6 +330,40 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( pose_with_covariance_stamped.header.frame_id = "map"; pose_with_covariance_stamped.pose.pose = tf2::toMsg(map_to_base_link); + // If latest_ekf_pose_ is older than seconds compared to current frame, it + // will not be published. + const rclcpp::Duration tolerance{ + static_cast(ekf_time_tolerance_), + static_cast((ekf_time_tolerance_ - std::floor(ekf_time_tolerance_)) * 1e9)}; + if (rclcpp::Time(latest_ekf_pose_.header.stamp) + tolerance < rclcpp::Time(msg.header.stamp)) { + RCLCPP_INFO( + this->get_logger(), + "latest_ekf_pose_ is older than %f seconds compared to current frame. " + "latest_ekf_pose_.header.stamp: %d.%d, msg.header.stamp: %d.%d", + ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, + msg.header.stamp.sec, msg.header.stamp.nanosec); + return; + } + + // If curr_pose differs from latest_ekf_pose_ by more than , it will not + // be published. + const geometry_msgs::msg::Pose curr_pose = pose_with_covariance_stamped.pose.pose; + const geometry_msgs::msg::Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; + const double diff_x = curr_pose.position.x - latest_ekf_pose.position.x; + const double diff_y = curr_pose.position.y - latest_ekf_pose.position.y; + const double diff_z = curr_pose.position.z - latest_ekf_pose.position.z; + const double diff_distance_squared = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z; + const double threshold = ekf_position_tolerance_ * ekf_position_tolerance_; + if (threshold < diff_distance_squared) { + RCLCPP_INFO( + this->get_logger(), + "curr_pose differs from latest_ekf_pose_ by more than %f m. " + "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", + ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, + latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); + return; + } + // ~5[m]: base_covariance // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) const double distance = std::sqrt(distance_squared); @@ -279,3 +375,22 @@ void ArTagBasedLocalizer::publish_pose_as_base_link( pose_pub_->publish(pose_with_covariance_stamped); } + +tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) +{ + cv::Mat rot(3, 3, CV_64FC1); + cv::Mat r_vec64; + marker.Rvec.convertTo(r_vec64, CV_64FC1); + cv::Rodrigues(r_vec64, rot); + cv::Mat tran64; + marker.Tvec.convertTo(tran64, CV_64FC1); + + tf2::Matrix3x3 tf_rot( + rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), + rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), + rot.at(2, 2)); + + tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + + return tf2::Transform(tf_rot, tf_orig); +} diff --git a/localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp similarity index 100% rename from localization/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp rename to localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer_node.cpp diff --git a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg similarity index 91% rename from localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg rename to localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg index bc8ba29755..5895d8ef65 100644 --- a/localization/ar_tag_based_localizer/doc_image/lanelet2_data_structure.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/lanelet2_data_structure.drawio.svg @@ -6,7 +6,7 @@ width="336px" height="336px" viewBox="-0.5 -0.5 336 336" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNj5swEIZ/DVJ7wzYBcsymu+2lUtVU6tmCCVg1ODLOhuyvr1kMBDsROdBk1eQQ4ddfzDPj8SQeWRf1V0l3+XeRAvewn9Ye+eJhHC1i/d0Ix1Ygy6gVMsnSVkKDsGFvYETfqHuWQjUaqITgiu3GYiLKEhI10qiU4jAethV8vOuOZuAIm4RyV/3NUpW3aoyjQf8GLMu7nVG4bHsK2g02llQ5TcXhRCLPHllLIVT7VNRr4A27jks77+VCb/9iEkp1zYRg0c54pXxvjDMvpo6dtVLsyxSaCb5Hng45U7DZ0aTpPWj3ai1XBdctpB/NciAV1BffCfWW6ggBUYCSRz2kmxAaOCY6UAfrMLDux+SnnDuRGv9m/doDAv1gKFwgEjpEkINET9GxBtM4aLVrA3DL6gbhLHyCMR8cu3zwGTx4DjqRQ8cNmLvSIfiOdGKHDvnYdJB/QzpLh07wsejYJ+uWdBb+dCaGMl0115duJZxWFUvOJV9IndtrEsCJgYszBnaaBE4Vex0vf85qs8MPwfTGPd/YOpq+xa0Se5mAmXR6a1nrBP7EQorKDJSz0LsPequvcwty3FI7ftERp8aeqJQUf2AtuJBaKUXZRPSWcW5JlLOsbNypHQRaf2ril+lKY2U6Cpam/NJxGN/MW1EqUyuhcKZ0EVkHInIPBDkTL2SOA4Ed8sfHId/Xuscu5G8Hnvz/mQhbmZ7YpeO1qcheKPh3mSi4wi33vD2xXVvcsPJauFX7jHBO04enz9j7Zy5okQUtcqCFLrNwBmRuKf/2OPkVLS3uIbldgnV/Jqx+6vYvmn16kdrWz4/jh3jymkNoHjfo5vC/Spt/hz+nyPNf</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">5ZhNb6MwEIZ/DdLuDWwC9Nhm2+5hV1ophz1beAJWDY6Mk5D++jXFfNmJkgNNqk0OEbz+wPPMeDzg4WVRv0qyyX8LCtxDPq09/MNDCCeJ/m+EQytEKGyFTDLaSsEgrNg7GNE36pZRqCYdlRBcsc1UTEVZQqomGpFS7Kfd1oJPn7ohGTjCKiXcVf8yqvJWTVA86D+BZXn35CB6aFsK0nU2llQ5oWI/kvCzh5dSCNVeFfUSeMOu49KOeznR2i9MQqkuGRAu2hE7wrfGOLMwdeislWJbUmgG+B5+2udMwWpD0qZ1r92rtVwVXN8F+tJdgFnTDqSCeiSZBb2CKEDJg+7StUYGjomOoIO1H1j3ffIx504kxr9ZP/eAQF8YCieIRA6RwEGih+hYg/M4SLVpA3DN6gbhLHzCKR+UuHzQETxoDjqxQ8cNmJvSweiGdBKHDv7adAL/inQeHDrh16Jj76xr0ln45zMxlPSxOb70XcpJVbH0wuQLdHKguQBGBi6OGNhpEjhRbDfNi8esNk/4I5heSc83sbamb3GrxFamYAaNTy1rntA/M5EiMgPlTPThg97qy9wSOG6pHb/o8FJTT1RKijdYCi6kVkpRNhG9ZpxbEuEsKxt3ao+B1p+aYGW60ng0DQWjlJ/aDtOTeS1KZWqlIJopXcTWhojdDYGPxAueY0Mgh/zhfsj3te6hC/nrgcf/fyZCVqbHdul4aSqyJwo/LxOFF7jllqcnsmuLK1ZeC7dqnxHOOH14eo99/OaCFlvQYgda5DKLZkDmlvLv95NfgweLe4Svl2Dd14RfpKQFkW/fXqS29fv9+CE5e8wFwTxu0LfDd5U2/w4fp/DzPw==</diagram></mxfile>" > @@ -138,12 +138,12 @@
- AR Tag(Front) + Landmark(Front)
- AR Tag(Front) + Landmark(Front) diff --git a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg similarity index 81% rename from localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg rename to localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg index a339951663..120a1f2aa3 100644 --- a/localization/ar_tag_based_localizer/doc_image/node_diagram.drawio.svg +++ b/localization/landmark_based_localizer/doc_image/node_diagram.drawio.svg @@ -6,20 +6,20 @@ width="721px" height="331px" viewBox="-0.5 -0.5 721 331" - content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1Zhdk5sgFIZ/TS6bEdF8XDYm2V5spzvdnWl75RBFZdaIg+Srv74QMcZAdt3EbKZeJHIAhec9nAP2oLfcPjCUJ99piNOebYXbHpz2bBtYliv+pGVXWhwwKA0xI6FqVBueyV9c9VTWFQlx0WjIKU05yZvGgGYZDnjDhhijm2aziKbNt+YoxprhOUCpbv1FQp6U1pE9rO3fMImT6s1gMC5rlqhqrGZSJCikmyMTnPWgxyjl5d1y6+FUwqu4lP3mZ2oPA2M442062GWHNUpXam5qXHxXTZbRVRZi2d7qwUlEM64EAbYqezSlbN8YWvtL2PWBqLGtMeN4e2RSA3vAdIk524kmqtZ2FSTlJVAVNzXyg0ckR7gHyoaUyvHhyTUIcaNYmLk4GpevP0X5BcXid4IKwcMWPguBnOsjFa4hmLD32JE0PWLleTN3LsYzKTijr/ioZhCM8CK6LV04aNJ1dLpjA1ynA7iuBvcHTyQ96+uK042Y1RHcHAWvYj0VH2U7n3ueie3IXsDB4Maeez+2cGhY0XO+IQX3N4QnfkDXiBGUBVgjikMR5VSRMp7QmGYondXWiUDCdr8V8n3hjyz0h25Vnm6Pa6c7VToLlSMWY2WCyibH8SZmhlPEyboZjU3UVNcnSsRrz7o+dEbNRxR0xQKsep2wPwyjlRxVnG/KQZYywbyFP6PZW7ztTniD4efwdqDb4G0PTxy9HJTGW3uQbTWFsyG8mXAjk3A5LfCHllGl45bwvYx9V5WkjkDd1xrKwu5Y0E61L+k0tb9o/Sk5vlh9IK6y07U+Mmz6CIQf9ZGqIY2iAl8rPzCGUcR8jmJ/IfO/n16Z9c/lH303sL+6yUzO6Qqy9dQEXENucrvITcAEFb9Gl7NUWf4uLAen+1PwmSyN8SlAS8yQT7KIXpNeuknnn5VetKzgtkvnhjzVfBAYua1i0AXxxYEG+d4XrFVoh2NdC6fzrVVrRx1rM31EGU4x12eMirw8rkdkK9f8JMeMiDdiuXrFY8X5Hj/VpomIGXEmqgKBY284HKelk4aoSA6RoxDnCJLFLzQXBigM5R7s8D8lS3G0m6dkIeN8IBH4IWFiOFTOdx4ijmTcL/rFOu4mfoxOvM2Wu4PTAGKKH874elkc/RAmzwmRX3Chf9DGGQ98QVvPrA44t40SrREY9+YywQsMASr4/3WgP03tAIwN/nSzhOTon0w6i2iOIbtU6t3Bb4zbmFQFNX+JcrGfQeHFm5n7fLI49R5jNOrKe0Sx/sZZps/6SzGc/QM=</diagram></mxfile>" + content="<mxfile><diagram id="z_GJx55S16-dNHi1224P" name="ページ1">1VnLjpswFP2aLBsFG0iybF7TxVQdaSq1XSEHDFgDGBnn1a+vHQwE8ExIQiYqm+BjA/Y51/fhDOA83j8xlIbfqYejARh5+wFcDAAwgA3Ej0QOOWIDMwcCRjw1qAJeyV+swJFCN8TDWW0gpzTiJK2DLk0S7PIahhiju/own0b1r6YowC3g1UVRG/1FPB7m6ASMK/wbJkFYfNmwp3lPjIrBaiVZiDy6O4HgcgDnjFKe38X7OY4keQUv+XOrd3rLiTGc8C4PKCG2KNqotal58UOxWEY3iYfl+NEAznyacCWIAVR7TiPKjoPh6HgJvD0RNbctZhzvTyA1sSdMY8zZQQxRvcBSJCkrgaq5qygvsfCEblthSKkclG+uiBA3igs9L2aLl2eUeDFibwKdoUzwAWyxXkOu9ZkK0xCcsHPckSg64Wo+X1orMZ9Zxhl9wyc9tjvBa/++7MJxnV2zza4x0rBr9sCu1WL3Bw8lfaOvG053Ylkn7KbIfRMbKruU3NVqPteROwFraNt3Nl37ceTCsWZPr/iOZNzZER46Lt0iRlDi4hal2BN+TjUp4yENaIKiZYXOBCfs8Ftxfmz8kY3h2Crai/1p7+KgWu+yyhELsIKgwuQ8PuSZ4Qhxsq37Yx1r6tEXSsRnK+Nv6APNSf0VGd0wF6unGtyX0+gkR+Hp63KQWIaYj+hPaPIR36AXvo3x5/BtQqvGNxg3DD2fVIvv1ovAqC4cgPBuwk10wqU0wxdto0LHPeFHGYeWakkdDXVfaSgbh1NBe9U+Z6eu/VX7T8nxZTQ0xJU/dKuNjOs2AuGlNlIMpL6f4VvlN7RuFDGHo8BZywzAiW6M++8FoHY+cLz6CU1mcwcBTWyyNLHJ6iM2GTpS8Zt/PZcqzD+ES7uZoRqfyaXWP7koxgw5JPHpLeGln3D+WeGlFRWsbuFcE6fqLzImVicfdIV/MaFGvvOCdXLtcNrWwuw9tepsqNPWSkUxhSPM2ytGWZoX7D7Zyz0/SzEj4otY7l7xWlHh45cKmgmfESSiyxV0HIGyoJZG6qEsLD1HJgoJkgQ/aSoAKIA8Byt/FyQOxFIispZ+3pUUOB5hYjpUrnflIY6k38+G2Tbox39MGtYGZHbQdCA6/2FOb5fFbFdhsk7wnYwL/d0uxljya3S1zKLAua+X6EyBNjePVKXvCC5clPH/q65vxnfDmGqM6m5RyWyfnPTm1kxNiCkkfIDxaHOZSHk2J0apSGqQd3VG85iDi6b1aF3S/azH1nEq88OL664zxxdGPd85l+7oy7eyYtOWb91C9QPTpuYRILw2bbKbxynWeNhX4iSa1XF4Prz6UwEu/wE=</diagram></mxfile>" > - - + + - AR Tag Based - Localizer + Landmark Based + Localizer - + - Other Autoware - packages + Other Autoware + packages @@ -74,7 +74,7 @@ - /tag_tf_caster + /landmark_tf_caster @@ -82,5 +82,11 @@ /lanelet2_map_loader + + + + + /ekf_pose_with_covariance + diff --git a/localization/ar_tag_based_localizer/doc_image/principle.png b/localization/landmark_based_localizer/doc_image/principle.png similarity index 100% rename from localization/ar_tag_based_localizer/doc_image/principle.png rename to localization/landmark_based_localizer/doc_image/principle.png diff --git a/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt new file mode 100644 index 0000000000..47899d716e --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(landmark_tf_caster) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +find_package(OpenCV REQUIRED) + +ament_auto_add_executable(landmark_tf_caster + src/landmark_tf_caster_node.cpp + src/landmark_tf_caster_core.cpp +) +target_include_directories(landmark_tf_caster + SYSTEM PUBLIC + ${OpenCV_INCLUDE_DIRS} +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml b/localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml similarity index 100% rename from localization/ar_tag_based_localizer/config/tag_tf_caster.param.yaml rename to localization/landmark_based_localizer/landmark_tf_caster/config/landmark_tf_caster.param.yaml diff --git a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp similarity index 84% rename from localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp rename to localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp index 5b10fe5b1d..0eb6706a0b 100644 --- a/localization/ar_tag_based_localizer/include/ar_tag_based_localizer/tag_tf_caster_core.hpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/include/landmark_tf_caster/landmark_tf_caster_core.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ -#define AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#ifndef LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ +#define LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ #include @@ -26,10 +26,10 @@ #include -class TagTfCaster : public rclcpp::Node +class LandmarkTfCaster : public rclcpp::Node { public: - TagTfCaster(); + LandmarkTfCaster(); private: void map_bin_callback(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg); @@ -46,4 +46,4 @@ class TagTfCaster : public rclcpp::Node rclcpp::Subscription::SharedPtr map_bin_sub_; }; -#endif // AR_TAG_BASED_LOCALIZER__TAG_TF_CASTER_CORE_HPP_ +#endif // LANDMARK_TF_CASTER__LANDMARK_TF_CASTER_CORE_HPP_ diff --git a/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml new file mode 100644 index 0000000000..eeaba555cb --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/launch/landmark_tf_caster.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_tf_caster/package.xml b/localization/landmark_based_localizer/landmark_tf_caster/package.xml new file mode 100644 index 0000000000..9740e26e2d --- /dev/null +++ b/localization/landmark_based_localizer/landmark_tf_caster/package.xml @@ -0,0 +1,25 @@ + + + + landmark_tf_caster + 0.0.0 + The landmark_tf_caster package + Shintaro Sakoda + Masahiro Sakamoto + Apache License 2.0 + + ament_cmake + autoware_cmake + + autoware_auto_mapping_msgs + geometry_msgs + lanelet2_extension + rclcpp + tf2_eigen + tf2_geometry_msgs + tf2_ros + + + ament_cmake + + diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp similarity index 93% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp index 427bf51305..801e036415 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_core.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_core.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -26,7 +26,7 @@ #include #endif -TagTfCaster::TagTfCaster() : Node("tag_tf_caster") +LandmarkTfCaster::LandmarkTfCaster() : Node("landmark_tf_caster") { // Parameters volume_threshold_ = this->declare_parameter("volume_threshold", 1e-5); @@ -38,11 +38,11 @@ TagTfCaster::TagTfCaster() : Node("tag_tf_caster") // Subscribers map_bin_sub_ = this->create_subscription( "~/input/lanelet2_map", rclcpp::QoS(10).durability(rclcpp::DurabilityPolicy::TransientLocal), - std::bind(&TagTfCaster::map_bin_callback, this, std::placeholders::_1)); + std::bind(&LandmarkTfCaster::map_bin_callback, this, std::placeholders::_1)); RCLCPP_INFO(this->get_logger(), "finish setup"); } -void TagTfCaster::map_bin_callback( +void LandmarkTfCaster::map_bin_callback( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg) { RCLCPP_INFO_STREAM(this->get_logger(), "msg->format_version: " << msg->format_version); @@ -60,7 +60,7 @@ void TagTfCaster::map_bin_callback( } } -void TagTfCaster::publish_tf(const lanelet::Polygon3d & poly) +void LandmarkTfCaster::publish_tf(const lanelet::Polygon3d & poly) { // Get marker_id const std::string marker_id = poly.attributeOr("marker_id", "none"); diff --git a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp similarity index 85% rename from localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp rename to localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp index ce83c572d6..4c34e593e7 100644 --- a/localization/ar_tag_based_localizer/src/tag_tf_caster_node.cpp +++ b/localization/landmark_based_localizer/landmark_tf_caster/src/landmark_tf_caster_node.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ar_tag_based_localizer/tag_tf_caster_core.hpp" +#include "landmark_tf_caster/landmark_tf_caster_core.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); } diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index 23e71f2a84..4a186bdf95 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -5,6 +5,7 @@ 0.1.0 ros node for monitoring localization error Yamato Ando + Koji Minoda Masahiro Sakamoto Apache License 2.0 diff --git a/localization/ndt_scan_matcher/README.md b/localization/ndt_scan_matcher/README.md index fcbe4804ca..c5a14d94eb 100644 --- a/localization/ndt_scan_matcher/README.md +++ b/localization/ndt_scan_matcher/README.md @@ -57,6 +57,7 @@ One optional function is regularization. Please see the regularization chapter i | Name | Type | Description | | --------------------------------------- | ------ | ----------------------------------------------------------------------------------------------- | | `base_frame` | string | Vehicle reference frame | +| `ndt_base_frame` | string | NDT reference frame | | `input_sensor_points_queue_size` | int | Subscriber queue size | | `trans_epsilon` | double | The maximum difference between two consecutive transformations in order to consider convergence | | `step_size` | double | The newton line search maximum step length | 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 b5affd72b2..c8afa9ea1a 100644 --- a/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml +++ b/localization/ndt_scan_matcher/config/ndt_scan_matcher.param.yaml @@ -6,6 +6,9 @@ # Vehicle reference frame base_frame: "base_link" + # NDT reference frame + ndt_base_frame: "ndt_base_link" + # Subscriber queue size input_sensor_points_queue_size: 1 diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp index 933375d579..5c37cef36c 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/util_func.hpp @@ -33,6 +33,7 @@ #include #include #include +#include #include // ref by http://takacity.blog.fc2.com/blog-entry-69.html @@ -86,4 +87,8 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); +void output_pose_with_cov_to_log( + const rclcpp::Logger logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); + #endif // NDT_SCAN_MATCHER__UTIL_FUNC_HPP_ 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 103df19c2b..7bb2b79e88 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -97,31 +97,30 @@ NDTScanMatcher::NDTScanMatcher() inversion_vector_threshold_(-0.9), oscillation_threshold_(10), output_pose_covariance_(), - regularization_enabled_(declare_parameter("regularization_enabled", false)), + regularization_enabled_(declare_parameter("regularization_enabled")), estimate_scores_for_degrounded_scan_( - declare_parameter("estimate_scores_for_degrounded_scan", false)), - z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal", 0.8)) + declare_parameter("estimate_scores_for_degrounded_scan")), + z_margin_for_ground_removal_(declare_parameter("z_margin_for_ground_removal")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; - int points_queue_size = - static_cast(this->declare_parameter("input_sensor_points_queue_size", 0)); + int points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); points_queue_size = std::max(points_queue_size, 0); RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size); - base_frame_ = this->declare_parameter("base_frame", base_frame_); + base_frame_ = this->declare_parameter("base_frame"); RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); - ndt_base_frame_ = this->declare_parameter("ndt_base_frame", ndt_base_frame_); + ndt_base_frame_ = this->declare_parameter("ndt_base_frame"); RCLCPP_INFO(get_logger(), "ndt_base_frame_id: %s", ndt_base_frame_.c_str()); pclomp::NdtParams ndt_params{}; ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); - ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); + ndt_params.max_iterations = this->declare_parameter("max_iterations"); + 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 = static_cast(this->declare_parameter("regularization_scale_factor")); @@ -132,24 +131,20 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, ndt_params.max_iterations); - int converged_param_type_tmp = - static_cast(this->declare_parameter("converged_param_type", 0)); + int converged_param_type_tmp = this->declare_parameter("converged_param_type"); converged_param_type_ = static_cast(converged_param_type_tmp); - converged_param_transform_probability_ = this->declare_parameter( - "converged_param_transform_probability", converged_param_transform_probability_); - converged_param_nearest_voxel_transformation_likelihood_ = this->declare_parameter( - "converged_param_nearest_voxel_transformation_likelihood", - converged_param_nearest_voxel_transformation_likelihood_); + converged_param_transform_probability_ = + this->declare_parameter("converged_param_transform_probability"); + converged_param_nearest_voxel_transformation_likelihood_ = + this->declare_parameter("converged_param_nearest_voxel_transformation_likelihood"); - lidar_topic_timeout_sec_ = - this->declare_parameter("lidar_topic_timeout_sec", lidar_topic_timeout_sec_); + lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); - initial_pose_timeout_sec_ = - this->declare_parameter("initial_pose_timeout_sec", initial_pose_timeout_sec_); + initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); - initial_pose_distance_tolerance_m_ = this->declare_parameter( - "initial_pose_distance_tolerance_m", initial_pose_distance_tolerance_m_); + initial_pose_distance_tolerance_m_ = + this->declare_parameter("initial_pose_distance_tolerance_m"); std::vector output_pose_covariance = this->declare_parameter>("output_pose_covariance"); @@ -157,8 +152,7 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_[i] = output_pose_covariance[i]; } - initial_estimate_particles_num_ = static_cast( - this->declare_parameter("initial_estimate_particles_num", initial_estimate_particles_num_)); + initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); rclcpp::CallbackGroup::SharedPtr initial_pose_callback_group; initial_pose_callback_group = @@ -783,6 +777,8 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ return geometry_msgs::msg::PoseWithCovarianceStamped(); } + output_pose_with_cov_to_log(get_logger(), "align_using_monte_carlo_input", initial_pose_with_cov); + // generateParticle const auto initial_poses = create_random_pose_array(initial_pose_with_cov, initial_estimate_particles_num_); @@ -820,5 +816,9 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_using_monte_ result_pose_with_cov_msg.header.frame_id = map_frame_; result_pose_with_cov_msg.pose.pose = best_particle_ptr->result_pose; + output_pose_with_cov_to_log( + get_logger(), "align_using_monte_carlo_output", result_pose_with_cov_msg); + RCLCPP_INFO_STREAM(get_logger(), "best_score," << best_particle_ptr->score); + return result_pose_with_cov_msg; } diff --git a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp index 59ecedc18c..f09b71523e 100644 --- a/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp +++ b/localization/ndt_scan_matcher/src/pose_array_interpolator.cpp @@ -51,6 +51,7 @@ PoseArrayInterpolator::PoseArrayInterpolator( // all validations must be true if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + success_ = false; RCLCPP_WARN(logger_, "Validation error."); } } diff --git a/localization/ndt_scan_matcher/src/util_func.cpp b/localization/ndt_scan_matcher/src/util_func.cpp index d947c56dfe..52cb7844ab 100644 --- a/localization/ndt_scan_matcher/src/util_func.cpp +++ b/localization/ndt_scan_matcher/src/util_func.cpp @@ -287,3 +287,24 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin const double dz = p1.z - p2.z; return std::sqrt(dx * dx + dy * dy + dz * dz); } + +void output_pose_with_cov_to_log( + const rclcpp::Logger logger, const std::string & prefix, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) +{ + const Eigen::Map covariance = + make_eigen_covariance(pose_with_cov.pose.covariance); + const geometry_msgs::msg::Pose pose = pose_with_cov.pose.pose; + geometry_msgs::msg::Vector3 rpy = get_rpy(pose); + rpy.x = rpy.x * 180.0 / M_PI; + rpy.y = rpy.y * 180.0 / M_PI; + rpy.z = rpy.z * 180.0 / M_PI; + + RCLCPP_INFO_STREAM( + logger, std::fixed << prefix << "," << pose.position.x << "," << pose.position.y << "," + << pose.position.z << "," << pose.orientation.x << "," << pose.orientation.y + << "," << pose.orientation.z << "," << pose.orientation.w << "," << rpy.x + << "," << rpy.y << "," << rpy.z << "," << covariance(0, 0) << "," + << covariance(1, 1) << "," << covariance(2, 2) << "," << covariance(3, 3) + << "," << covariance(4, 4) << "," << covariance(5, 5)); +} diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 2d1867b8cd..97a0443195 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -13,4 +13,5 @@ ament_target_dependencies(stop_filter) ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/localization/stop_filter/config/stop_filter.param.yaml b/localization/stop_filter/config/stop_filter.param.yaml new file mode 100644 index 0000000000..ded090b75b --- /dev/null +++ b/localization/stop_filter/config/stop_filter.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + vx_threshold: 0.1 # [m/s] + wz_threshold: 0.02 # [rad/s] diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 36a66a2c14..0ea92d26c9 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -1,6 +1,5 @@ - - + @@ -10,7 +9,6 @@ - - + diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 111d460be7..ac0960b8cb 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -27,8 +27,8 @@ using std::placeholders::_1; StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) : rclcpp::Node(node_name, node_options) { - vx_threshold_ = declare_parameter("vx_threshold", 0.01); - wz_threshold_ = declare_parameter("wz_threshold", 0.01); + vx_threshold_ = declare_parameter("vx_threshold"); + wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); diff --git a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt index cd54a3c5f2..164f280bec 100644 --- a/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt +++ b/localization/yabloc/yabloc_pose_initializer/CMakeLists.txt @@ -14,24 +14,6 @@ find_package(PCL REQUIRED COMPONENTS common kdtree) # Sophus find_package(Sophus REQUIRED) -# =================================================== -# Download DNN model -include(download.cmake) - -# =================================================== -# Clear ${PYTHON_EXECUTABLE} defined by mrt_cmake_module so that rosidl_generate_interfaces can look for it properly -unset(PYTHON_EXECUTABLE) -message(STATUS "PYTHON_EXECUTABLE: ${PYTHON_EXECUTABLE}") - -# =================================================== -# Service -rosidl_generate_interfaces(${PROJECT_NAME} - "srv/SemanticSegmentation.srv" - DEPENDENCIES - std_msgs - sensor_msgs -) - # =================================================== # Executable # Camera @@ -40,26 +22,12 @@ ament_auto_add_executable(${TARGET} src/camera/lane_image.cpp src/camera/marker_module.cpp src/camera/projector_module.cpp + src/camera/semantic_segmentation.cpp src/camera/camera_pose_initializer_core.cpp src/camera/camera_pose_initializer_node.cpp) target_include_directories(${TARGET} PUBLIC include) target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) target_link_libraries(${TARGET} ${PCL_LIBRARIES} Sophus::Sophus) -# ros2idl typesupport -if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${TARGET} ${PROJECT_NAME} "rosidl_typesupport_cpp") -else() - rosidl_get_typesupport_target(cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${TARGET} "${cpp_typesupport_target}") -endif() - -# Semantic segmentation -install(PROGRAMS - src/semantic_segmentation/semantic_segmentation_core.py - src/semantic_segmentation/semantic_segmentation_server.py - DESTINATION lib/${PROJECT_NAME} -) - # =================================================== -ament_auto_package(INSTALL_TO_SHARE config data launch) +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/localization/yabloc/yabloc_pose_initializer/README.md b/localization/yabloc/yabloc_pose_initializer/README.md index 4e3c0a0363..907b79c145 100644 --- a/localization/yabloc/yabloc_pose_initializer/README.md +++ b/localization/yabloc/yabloc_pose_initializer/README.md @@ -1,27 +1,24 @@ # yabloc_pose_initializer -This package contains some nodes related to initial pose estimation. +This package contains a node related to initial pose estimation. - [camera_pose_initializer](#camera_pose_initializer) -- [semantic_segmentation_server](#semantic_segmentation_server) -Ideally, this package downloads a pre-trained semantic segmentation model during the build and loads it at runtime for initialization. -However, to handle cases where network connectivity is not available at build time, **the default behavior is not to download the model during build.** -Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. +This package requires the pre-trained semantic segmentation model for runtime. This model is usually downloaded by `ansible` during env preparation phase of the [installation](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/). +It is also possible to download it manually. Even if the model is not downloaded, initialization will still complete, but the accuracy may be compromised. - - -To download the model, please specify `--cmake-args -DDOWNLOAD_ARTIFACTS=ON` to the build command. +To download and extract the model manually: ```bash -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DDOWNLOAD_ARTIFACTS=ON --packages-select yabloc_pose_initializer +$ mkdir -p ~/autoware_data/yabloc_pose_initializer/ +$ wget -P ~/autoware_data/yabloc_pose_initializer/ \ + https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz +$ tar xzf ~/autoware_data/yabloc_pose_initializer/resources.tar.gz -C ~/autoware_data/yabloc_pose_initializer/ ``` -For detailed information about the downloaded contents, please consult the `download.cmake` file in this package. - ## Note -This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded when you build. +This package makes use of external code. The trained files are provided by apollo. The trained files are automatically downloaded during env preparation. Original model URL @@ -71,21 +68,3 @@ Converted model URL | Name | Type | Description | | ------------------ | --------------------------------------------------------- | ------------------------------- | | `yabloc_align_srv` | `tier4_localization_msgs::srv::PoseWithCovarianceStamped` | initial pose estimation request | - -### Clients - -| Name | Type | Description | -| --------------------------- | ---------------------------------------------------- | ----------------------------- | -| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | - -## semantic_segmentation_server - -### Purpose - -- This node performs semantic segmentation. - -### Services - -| Name | Type | Description | -| --------------------------- | ---------------------------------------------------- | ----------------------------- | -| `semantic_segmentation_srv` | `yabloc_pose_initializer::srv::SemanticSegmentation` | semantic segmentation request | diff --git a/localization/yabloc/yabloc_pose_initializer/download.cmake b/localization/yabloc/yabloc_pose_initializer/download.cmake deleted file mode 100644 index 6ce997b397..0000000000 --- a/localization/yabloc/yabloc_pose_initializer/download.cmake +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 2023 the 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. - -set(DOWNLOAD_ARTIFACTS OFF CACHE BOOL "enable artifacts download") - -set(DATA_URL "https://s3.ap-northeast-2.wasabisys.com/pinto-model-zoo/136_road-segmentation-adas-0001/resources.tar.gz") -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -set(FILE_HASH 146ed8af689a30b898dc5369870c40fb) -set(FILE_NAME "resources.tar.gz") - -function(download_and_extract) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - if(DOWNLOAD_ARTIFACTS) - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DATA_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - else() - message(WARNING "Skipped download for ${FILE_NAME} (enable by setting DOWNLOAD_ARTIFACTS)") - file(MAKE_DIRECTORY "${DATA_PATH}") - return() - endif() - endif() - - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() - - execute_process(COMMAND - ${CMAKE_COMMAND} -E - tar xzf "${DATA_PATH}/${FILE_NAME}" WORKING_DIRECTORY "${DATA_PATH}") -endfunction() - -download_and_extract() diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp index ccb55e79b8..917a6ecaaf 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/camera_pose_initializer.hpp @@ -18,10 +18,10 @@ #include "yabloc_pose_initializer/camera/lane_image.hpp" #include "yabloc_pose_initializer/camera/marker_module.hpp" #include "yabloc_pose_initializer/camera/projector_module.hpp" +#include "yabloc_pose_initializer/camera/semantic_segmentation.hpp" #include #include -#include #include #include @@ -42,7 +42,6 @@ class CameraPoseInitializer : public rclcpp::Node using Image = sensor_msgs::msg::Image; using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using RequestPoseAlignment = tier4_localization_msgs::srv::PoseWithCovarianceStamped; - using SegmentationSrv = yabloc_pose_initializer::srv::SemanticSegmentation; CameraPoseInitializer(); @@ -57,12 +56,13 @@ class CameraPoseInitializer : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_image_; rclcpp::Service::SharedPtr align_server_; - rclcpp::Client::SharedPtr segmentation_client_; rclcpp::CallbackGroup::SharedPtr service_callback_group_; std::optional latest_image_msg_{std::nullopt}; lanelet::ConstLanelets const_lanelets_; + std::unique_ptr semantic_segmentation_{nullptr}; + void on_map(const HADMapBin & msg); void on_service( const RequestPoseAlignment::Request::SharedPtr, diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp index cc084990a0..071d55482f 100644 --- a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/projector_module.hpp @@ -33,7 +33,7 @@ class ProjectorModule bool define_project_func(); - cv::Mat project_image(const sensor_msgs::msg::Image & image_msg); + cv::Mat project_image(const cv::Mat & mask_image); private: common::CameraInfoSubscriber info_; diff --git a/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp new file mode 100644 index 0000000000..061d0548c6 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/include/yabloc_pose_initializer/camera/semantic_segmentation.hpp @@ -0,0 +1,46 @@ +// 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 YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ +#define YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ +#include +#include + +#include +#include +#include + +class SemanticSegmentation +{ +public: + explicit SemanticSegmentation(const std::string & model_path); + + cv::Mat inference(const cv::Mat & image, double score_threshold = 0.5); + + static void print_error_message(const rclcpp::Logger & logger); + +private: + cv::Mat make_blob(const cv::Mat & image); + + cv::Mat convert_blob_to_image(const cv::Mat & blob); + + cv::Mat normalize(const cv::Mat & mask, double score_threshold = 0.5); + + cv::Mat draw_overlay(const cv::Mat & image, const cv::Mat & segmentation); + + struct Impl; + std::shared_ptr impl_{nullptr}; +}; + +#endif // YABLOC_POSE_INITIALIZER__CAMERA__SEMANTIC_SEGMENTATION_HPP_ diff --git a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml index a1d5d59b46..83c2c8fe09 100644 --- a/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml +++ b/localization/yabloc/yabloc_pose_initializer/launch/yabloc_pose_initializer.launch.xml @@ -1,17 +1,14 @@ + + + + - - - - - - - diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp index 5fab976332..19d3c8d88f 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/camera_pose_initializer_core.cpp @@ -20,6 +20,10 @@ #include #include +#include + +#include + namespace yabloc { CameraPoseInitializer::CameraPoseInitializer() @@ -39,17 +43,17 @@ CameraPoseInitializer::CameraPoseInitializer() sub_map_ = create_subscription("~/input/vector_map", map_qos, on_map); sub_image_ = create_subscription("~/input/image_raw", 10, on_image); - // Client - segmentation_client_ = create_client( - "~/semantic_segmentation_srv", rmw_qos_profile_services_default, service_callback_group_); - // Server auto on_service = std::bind(&CameraPoseInitializer::on_service, this, _1, _2); align_server_ = create_service("~/yabloc_align_srv", on_service); - using namespace std::literals::chrono_literals; - while (!segmentation_client_->wait_for_service(1s) && rclcpp::ok()) { - RCLCPP_INFO_STREAM(get_logger(), "Waiting for " << segmentation_client_->get_service_name()); + const std::string model_path = declare_parameter("model_path"); + RCLCPP_INFO_STREAM(get_logger(), "segmentation model path: " + model_path); + if (std::filesystem::exists(model_path)) { + semantic_segmentation_ = std::make_unique(model_path); + } else { + semantic_segmentation_ = nullptr; + SemanticSegmentation::print_error_message(get_logger()); } } @@ -97,31 +101,20 @@ std::optional CameraPoseInitializer::estimate_pose( return std::nullopt; } - Image segmented_image; + cv::Mat segmented_image; { - // Call semantic segmentation service - auto request = std::make_shared(); - request->src_image = *latest_image_msg_.value(); - auto result_future = segmentation_client_->async_send_request(request); - using namespace std::literals::chrono_literals; - std::future_status status = result_future.wait_for(1000ms); - if (status == std::future_status::ready) { - const auto response = result_future.get(); - if (response->success) { - segmented_image = response->dst_image; - } else { - RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); - // NOTE: Even if the segmentation service fails, the function will still return the - // yaw_angle_rad as it is and complete the initialization. The service fails - // when the DNN model is not downloaded. Ideally, initialization should rely on - // segmentation, but this implementation allows initialization even in cases where network - // connectivity is not available. - return yaw_angle_rad; - } + if (semantic_segmentation_) { + const cv::Mat src_image = + cv_bridge::toCvCopy(*latest_image_msg_.value(), sensor_msgs::image_encodings::BGR8)->image; + segmented_image = semantic_segmentation_->inference(src_image); } else { - RCLCPP_ERROR_STREAM( - get_logger(), "segmentation service did not return within the expected time"); - return std::nullopt; + RCLCPP_WARN_STREAM(get_logger(), "segmentation service failed unexpectedly"); + // NOTE: Even if the segmentation service fails, the function will still return the + // yaw_angle_rad as it is and complete the initialization. The service fails + // when the DNN model is not downloaded. Ideally, initialization should rely on + // segmentation, but this implementation allows initialization even in cases where network + // connectivity is not available. + return yaw_angle_rad; } } diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp index 00f2da473b..86a403f68a 100644 --- a/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/projector_module.cpp @@ -36,10 +36,8 @@ cv::Point2i to_cv_point(const Eigen::Vector3f & v) return pt; } -cv::Mat ProjectorModule::project_image(const sensor_msgs::msg::Image & image_msg) +cv::Mat ProjectorModule::project_image(const cv::Mat & mask_image) { - cv::Mat mask_image = common::decompress_to_cv_mat(image_msg); - // project semantics on plane std::vector masks; cv::split(mask_image, masks); diff --git a/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp new file mode 100644 index 0000000000..760ada6f83 --- /dev/null +++ b/localization/yabloc/yabloc_pose_initializer/src/camera/semantic_segmentation.cpp @@ -0,0 +1,121 @@ +// 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 "yabloc_pose_initializer/camera/semantic_segmentation.hpp" + +#include +#include +#include +#include + +#include +#include + +struct SemanticSegmentation::Impl +{ + cv::dnn::Net net; +}; + +SemanticSegmentation::SemanticSegmentation(const std::string & model_path) +{ + impl_ = std::make_shared(); + impl_->net = cv::dnn::readNet(model_path); + impl_->net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV); + impl_->net.setPreferableTarget(cv::dnn::DNN_TARGET_CPU); +} + +cv::Mat SemanticSegmentation::make_blob(const cv::Mat & image) +{ + double scale = 1.0; + cv::Size size = cv::Size(896, 512); + cv::Scalar mean = cv::Scalar(0, 0, 0); + bool swap = true; + bool crop = false; + return cv::dnn::blobFromImage(image, scale, size, mean, swap, crop, CV_32F); +} + +cv::Mat SemanticSegmentation::convert_blob_to_image(const cv::Mat & blob) +{ + if (blob.size.dims() != 4) { + throw std::runtime_error("blob has invalid size"); + } + const int channels = blob.size[1]; + const int height = blob.size[2]; + const int width = blob.size[3]; + cv::Mat image = cv::Mat(height, width, CV_32FC4); + + for (int h = 0; h < height; ++h) { + for (int w = 0; w < width; ++w) { + cv::Vec4f vec4f; + for (int c = 0; c < channels; ++c) { + vec4f[c] = blob.at(c * height * width + h * width + w); + } + image.at(h, w) = vec4f; + } + } + return image; +} + +cv::Mat SemanticSegmentation::inference(const cv::Mat & image, double score_threshold) +{ + cv::Mat blob = make_blob(image); + impl_->net.setInput(blob); + std::vector output_layers = impl_->net.getUnconnectedOutLayersNames(); + std::vector masks; + impl_->net.forward(masks, output_layers); + + cv::Mat mask = masks[0]; + cv::Mat output = convert_blob_to_image(mask); + + cv::resize(output, output, cv::Size(image.cols, image.rows), 0, 0, cv::INTER_LINEAR); + + return normalize(output, score_threshold); +} + +cv::Mat SemanticSegmentation::normalize(const cv::Mat & mask, double score_threshold) +{ + std::vector masks; + cv::split(mask, masks); + std::vector bin_masks; + + for (size_t i = 1; i < masks.size(); ++i) { + cv::Mat bin_mask; + cv::threshold(masks[i], bin_mask, score_threshold, 255, cv::THRESH_BINARY_INV); + bin_mask.convertTo(bin_mask, CV_8UC1); + bin_masks.push_back(255 - bin_mask); + } + + cv::Mat result; + cv::merge(bin_masks, result); + return result; +} + +cv::Mat SemanticSegmentation::draw_overlay(const cv::Mat & image, const cv::Mat & segmentation) +{ + cv::Mat overlay_image = image.clone(); + return overlay_image * 0.5 + segmentation * 0.5; +} + +void SemanticSegmentation::print_error_message(const rclcpp::Logger & logger) +{ + const std::string ERROR_MESSAGE = + R"(The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. +Please check the README of yabloc_pose_initializer to know how download models.)"; + + std::istringstream stream(ERROR_MESSAGE); + std::string line; + while (std::getline(stream, line)) { + RCLCPP_ERROR_STREAM(logger, line); + } +} diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py deleted file mode 100755 index 70d116fe0b..0000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_core.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import copy - -import cv2 -import numpy as np - - -class SemanticSegmentationCore: - def __init__(self, model_path): - self.net_ = cv2.dnn.readNet(model_path) - self.net_.setPreferableBackend(cv2.dnn.DNN_BACKEND_OPENCV) - self.net_.setPreferableTarget(cv2.dnn.DNN_TARGET_CPU) - - def makeBlob(self, image: np.ndarray) -> np.ndarray: - scale = 1.0 - size = (896, 512) - mean = (0, 0, 0) - swap = True - crop = False - depth = cv2.CV_32F - return cv2.dnn.blobFromImage(image, scale, size, mean, swap, crop, depth) - - def inference(self, image: np.ndarray, score_threshold=0.5) -> np.ndarray: - blob = self.makeBlob(image) - self.net_.setInput(blob) - output_layers = self.net_.getUnconnectedOutLayersNames() - mask = self.net_.forward(output_layers)[0] - mask = np.squeeze(mask).transpose(1, 2, 0) - - mask = cv2.resize( - mask, dsize=(image.shape[1], image.shape[0]), interpolation=cv2.INTER_LINEAR - ) - - return self.__normalize(mask, score_threshold) - - def __normalize(self, mask, score_threshold=0.5) -> np.ndarray: - masks = cv2.split(mask)[1:] - bin_masks = [] - for mask in masks: - bin_mask = np.where(mask > score_threshold, 0, 1).astype("uint8") - bin_masks.append(255 - 255 * bin_mask) - return cv2.merge(bin_masks) - - def drawOverlay(self, image, segmentation) -> np.ndarray: - overlay_image = copy.deepcopy(image) - return cv2.addWeighted(overlay_image, 0.5, segmentation, 0.5, 1.0) - - -def main(): - print("This script is not intended to be run alone.") - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py b/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py deleted file mode 100755 index 03de9de572..0000000000 --- a/localization/yabloc/yabloc_pose_initializer/src/semantic_segmentation/semantic_segmentation_server.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env python3 - -# 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. - -import os -import sys - -from cv_bridge import CvBridge -import rclpy -from rclpy.node import Node -import semantic_segmentation_core as core -from sensor_msgs.msg import Image -from yabloc_pose_initializer.srv import SemanticSegmentation - -# cspell: ignore DDOWNLOAD -ERROR_MESSAGE = """\ -The yabloc_pose_initializer is not working correctly because the DNN model has not been downloaded correctly. -To download models, "-DDOWNLOAD_ARTIFACTS=ON" is required at build time. -Please see the README of yabloc_pose_initializer for more information.""" - - -class SemanticSegmentationServer(Node): - def __init__(self): - super().__init__("segmentation_server_node") - - model_path = self.declare_parameter("model_path", "").value - - self.get_logger().info("model path: " + model_path) - self.bridge_ = CvBridge() - - if os.path.exists(model_path): - self.dnn_ = core.SemanticSegmentationCore(model_path) - else: - self.dnn_ = None - self.__print_error_message() - - self.srv = self.create_service( - SemanticSegmentation, "semantic_segmentation_srv", self.on_service - ) - - def __print_error_message(self): - messages = ERROR_MESSAGE.split("\n") - for message in messages: - self.get_logger().error(message) - - def on_service(self, request, response): - if self.dnn_: - response.dst_image = self.__inference(request.src_image) - response.success = True - else: - self.__print_error_message() - response.success = False - response.dst_image = request.src_image - - return response - - def __inference(self, msg: Image): - stamp = msg.header.stamp - self.get_logger().info("Subscribed image: " + str(stamp)) - src_image = self.bridge_.imgmsg_to_cv2(msg) - - mask = self.dnn_.inference(src_image) - dst_msg = self.bridge_.cv2_to_imgmsg(mask) - dst_msg.encoding = "bgr8" - - return dst_msg - - -def main(): - rclpy.init(args=sys.argv) - - server_node = SemanticSegmentationServer() - rclpy.spin(server_node) - server_node.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv b/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv deleted file mode 100644 index 3daa611fd1..0000000000 --- a/localization/yabloc/yabloc_pose_initializer/srv/SemanticSegmentation.srv +++ /dev/null @@ -1,4 +0,0 @@ -sensor_msgs/Image src_image ---- -bool success -sensor_msgs/Image dst_image diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 816cfc418f..e39b6d66af 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -141,11 +141,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. -The node supports the following three types of coordinate systems: - -- MGRS -- LocalCartesianUTM -- local +Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. ### How to run diff --git a/map/map_projection_loader/CMakeLists.txt b/map/map_projection_loader/CMakeLists.txt index 8658ba172f..700d468ed4 100644 --- a/map/map_projection_loader/CMakeLists.txt +++ b/map/map_projection_loader/CMakeLists.txt @@ -42,6 +42,10 @@ if(BUILD_TESTING) test/test_node_load_local_cartesian_utm_from_yaml.test.py TIMEOUT "30" ) + add_launch_test( + test/test_node_load_transverse_mercator_from_yaml.test.py + TIMEOUT "30" + ) install(DIRECTORY test/data/ DESTINATION share/${PROJECT_NAME}/test/data/ diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 13808e6bf0..0b0c41821d 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -53,6 +53,20 @@ map_origin: altitude: 0.0 # [m] ``` +### Using TransverseMercator + +If you want to use Transverse Mercator projection, please specify the map origin as well. + +```yaml +# map_projector_info.yaml +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] +``` + ## Published Topics - ~/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Topic for defining map projector information diff --git a/map/map_projection_loader/src/map_projection_loader.cpp b/map/map_projection_loader/src/map_projection_loader.cpp index 74c340ca53..9fdba28860 100644 --- a/map/map_projection_loader/src/map_projection_loader.cpp +++ b/map/map_projection_loader/src/map_projection_loader.cpp @@ -31,16 +31,26 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_yaml(const std::string & fi if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::MGRS) { msg.vertical_datum = data["vertical_datum"].as(); msg.mgrs_grid = data["mgrs_grid"].as(); + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL_CARTESIAN_UTM) { msg.vertical_datum = data["vertical_datum"].as(); msg.map_origin.latitude = data["map_origin"]["latitude"].as(); msg.map_origin.longitude = data["map_origin"]["longitude"].as(); msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::TRANSVERSE_MERCATOR) { + msg.vertical_datum = data["vertical_datum"].as(); + msg.map_origin.latitude = data["map_origin"]["latitude"].as(); + msg.map_origin.longitude = data["map_origin"]["longitude"].as(); + msg.map_origin.altitude = data["map_origin"]["altitude"].as(); + } else if (msg.projector_type == tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { ; // do nothing + } else { throw std::runtime_error( - "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, and local"); + "Invalid map projector type. Currently supported types: MGRS, LocalCartesianUTM, " + "TransverseMercator, and local"); } return msg; } diff --git a/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml b/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml new file mode 100644 index 0000000000..de1febeb0a --- /dev/null +++ b/map/map_projection_loader/test/data/map_projector_info_transverse_mercator.yaml @@ -0,0 +1,6 @@ +projector_type: TransverseMercator +vertical_datum: WGS84 +map_origin: + latitude: 35.6762 # [deg] + longitude: 139.6503 # [deg] + altitude: 0.0 # [m] diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py new file mode 100644 index 0000000000..7883ae6aa3 --- /dev/null +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 + +# 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. + +import os +import unittest + +from ament_index_python import get_package_share_directory +import launch +from launch import LaunchDescription +from launch.logging import get_logger +from launch_ros.actions import Node +import launch_testing +import pytest +import rclpy +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSHistoryPolicy +from rclpy.qos import QoSProfile +from tier4_map_msgs.msg import MapProjectorInfo +import yaml + +logger = get_logger(__name__) + +YAML_FILE_PATH = "test/data/map_projector_info_transverse_mercator.yaml" + + +@pytest.mark.launch_test +def generate_test_description(): + map_projector_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + + map_projection_loader_node = Node( + package="map_projection_loader", + executable="map_projection_loader", + output="screen", + parameters=[ + { + "map_projector_info_path": map_projector_info_path, + "lanelet2_map_path": "", + "use_local_projector": False, + }, + ], + ) + + context = {} + + return ( + LaunchDescription( + [ + map_projection_loader_node, + # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ), + ] + ), + context, + ) + + +class TestLoadUTMFromYaml(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context for the test node + rclpy.init() + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + rclpy.shutdown() + + def setUp(self): + # Create a ROS node for tests + self.test_node = rclpy.create_node("test_node") + self.evaluation_time = 0.2 # 200ms + self.received_message = None + + def tearDown(self): + self.test_node.destroy_node() + + def callback(self, msg): + self.received_message = msg + + @staticmethod + def print_message(stat): + logger.debug("===========================") + logger.debug(stat) + + def test_node_link(self): + # Create custom QoS profile for subscription + custom_qos_profile = QoSProfile( + depth=1, + history=QoSHistoryPolicy.KEEP_LAST, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + ) + + # Create subscription to map_projector_info topic + subscription = self.test_node.create_subscription( + MapProjectorInfo, + "/map/map_projector_info", + self.callback, + custom_qos_profile, + ) + + # Give time for the message to be received and processed + rclpy.spin_until_future_complete( + self.test_node, rclpy.task.Future(), timeout_sec=self.evaluation_time + ) + + # Load the yaml file directly + map_projector_info_path = os.path.join( + get_package_share_directory("map_projection_loader"), YAML_FILE_PATH + ) + with open(map_projector_info_path) as f: + yaml_data = yaml.load(f, Loader=yaml.FullLoader) + + # Test if message received + self.assertIsNotNone( + self.received_message, "No message received on map_projector_info topic" + ) + self.assertEqual(self.received_message.projector_type, yaml_data["projector_type"]) + self.assertEqual(self.received_message.vertical_datum, yaml_data["vertical_datum"]) + self.assertEqual( + self.received_message.map_origin.latitude, yaml_data["map_origin"]["latitude"] + ) + self.assertEqual( + self.received_message.map_origin.longitude, yaml_data["map_origin"]["longitude"] + ) + + self.test_node.destroy_subscription(subscription) + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/perception/cluster_merger/include/cluster_merger/node.hpp b/perception/cluster_merger/include/cluster_merger/node.hpp index 8da5999f00..f36116378a 100644 --- a/perception/cluster_merger/include/cluster_merger/node.hpp +++ b/perception/cluster_merger/include/cluster_merger/node.hpp @@ -19,7 +19,7 @@ #include "message_filters/sync_policies/approximate_time.h" #include "message_filters/synchronizer.h" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_perception_msgs/msg/detected_objects_with_feature.hpp" diff --git a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp index 161a4d19b9..e38b747a6c 100644 --- a/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp +++ b/perception/crosswalk_traffic_light_estimator/include/crosswalk_traffic_light_estimator/node.hpp @@ -15,9 +15,6 @@ #ifndef CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ #define CROSSWALK_TRAFFIC_LIGHT_ESTIMATOR__NODE_HPP_ -#include -#include -#include #include #include #include @@ -32,6 +29,7 @@ #include #include +#include #include #include #include diff --git a/perception/crosswalk_traffic_light_estimator/src/node.cpp b/perception/crosswalk_traffic_light_estimator/src/node.cpp index 62c8ac1cb7..e45e3494da 100644 --- a/perception/crosswalk_traffic_light_estimator/src/node.cpp +++ b/perception/crosswalk_traffic_light_estimator/src/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "crosswalk_traffic_light_estimator/node.hpp" +#include #include #include diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp index d1d3cf9e90..600da31df5 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp @@ -17,14 +17,13 @@ #include "utils/utils.hpp" -#include -#include #include #include #include #include +#include #include #include diff --git a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp index b3ab5d104a..b9384e0f70 100644 --- a/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp +++ b/perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp @@ -17,8 +17,6 @@ #include "utils/utils.hpp" -#include -#include #include #include diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 2e393f27c3..3b78ae449e 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -14,6 +14,8 @@ #include "detected_object_filter/object_lanelet_filter.hpp" +#include +#include #include #include diff --git a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp index 3f383ce3d6..7c0609ee1c 100644 --- a/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp +++ b/perception/elevation_map_loader/include/elevation_map_loader/elevation_map_loader_node.hpp @@ -19,8 +19,6 @@ #include #include #include -#include -#include #include #include @@ -30,6 +28,7 @@ #include #include +#include #include #include diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 096e084a70..425af519a8 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -20,6 +20,8 @@ #include #include #include +#include +#include #include #include diff --git a/perception/euclidean_cluster/config/compare_map.param.yaml b/perception/euclidean_cluster/config/compare_map.param.yaml deleted file mode 100644 index 3dd303464a..0000000000 --- a/perception/euclidean_cluster/config/compare_map.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - - # distance threshold for compare compare - distance_threshold: 0.5 - - # publish voxelized map pointcloud for debug - publish_debug_pcd: False - - # use dynamic map loading - use_dynamic_map_loading: True - - # time interval to check dynamic map loading - timer_interval_ms: 100 - - # distance threshold for dynamic map update - map_update_distance_threshold: 10.0 - - # radius map for dynamic map loading - map_loader_radius: 150.0 diff --git a/perception/euclidean_cluster/config/outlier.param.yaml b/perception/euclidean_cluster/config/outlier.param.yaml deleted file mode 100644 index 1962fba1f3..0000000000 --- a/perception/euclidean_cluster/config/outlier.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.3 - voxel_size_y: 0.3 - voxel_size_z: 100.0 - voxel_points_threshold: 3 diff --git a/perception/euclidean_cluster/config/voxel_grid.param.yaml b/perception/euclidean_cluster/config/voxel_grid.param.yaml deleted file mode 100644 index 3ff32bfbb7..0000000000 --- a/perception/euclidean_cluster/config/voxel_grid.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - input_frame: base_link - output_frame: base_link - voxel_size_x: 0.15 - voxel_size_y: 0.15 - voxel_size_z: 0.15 diff --git a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml index 2f3de2b789..26b027f007 100644 --- a/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml +++ b/perception/euclidean_cluster/config/voxel_grid_based_euclidean_cluster.param.yaml @@ -7,9 +7,11 @@ max_cluster_size: 3000 use_height: false input_frame: "base_link" - max_x: 70.0 - min_x: -70.0 - max_y: 70.0 - min_y: -70.0 - max_z: 4.5 - min_z: -4.5 + + # low height crop box filter param + max_x: 200.0 + min_x: -200.0 + max_y: 200.0 + min_y: -200.0 + max_z: 2.0 + min_z: -10.0 diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py index db86bbf80d..66c25396a6 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.py @@ -35,61 +35,35 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", - plugin="pointcloud_preprocessor::VoxelGridDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), + namespace=ns, + plugin="pointcloud_preprocessor::CropBoxFilterComponent", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "voxel_grid_filtered/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), - ("map", LaunchConfiguration("input_map")), - ("output", "compare_map_filtered/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[ - { - "distance_threshold": 0.5, - "timer_interval_ms": 100, - "use_dynamic_map_loading": True, - "downsize_ratio_z_axis": 0.5, - "map_update_distance_threshold": 10.0, - "map_loader_radius": 150.0, - "publish_debug_pcd": True, - "input_frame": "map", - } + ("output", "low_height/pointcloud"), ], + parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_euclidean_cluster_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "compare_map_filtered/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], ) - disuse_map_euclidean_cluster_component = ComposableNode( + disuse_low_height_euclidean_component = ComposableNode( package=pkg, plugin="euclidean_cluster::EuclideanClusterNode", name=AnonName("euclidean_cluster"), remappings=[ - ("input", "voxel_grid_filtered/pointcloud"), + ("input", LaunchConfiguration("input_pointcloud")), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("euclidean_param_path")], @@ -100,26 +74,26 @@ def load_composable_node_param(param_path): namespace=ns, package="rclcpp_components", executable="component_container", - composable_node_descriptions=[voxel_grid_filter_component], + composable_node_descriptions=[], output="screen", ) - use_map_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition(LaunchConfiguration("use_pointcloud_map")), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_loader = LoadComposableNodes( - composable_node_descriptions=[disuse_map_euclidean_cluster_component], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=UnlessCondition(LaunchConfiguration("use_pointcloud_map")), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) - return [container, use_map_loader, disuse_map_loader] + return [container, use_low_height_pointcloud_loader, disuse_low_height_pointcloud_loader] def generate_launch_description(): @@ -131,14 +105,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml index 7159fb4c42..fd1ea82bef 100644 --- a/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/euclidean_cluster.launch.xml @@ -3,16 +3,14 @@ - - + - - + diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py index 82ce5605b6..00bcd4422b 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.py @@ -16,10 +16,8 @@ from launch.actions import DeclareLaunchArgument from launch.actions import OpaqueFunction from launch.conditions import IfCondition -from launch.substitutions import AndSubstitution -from launch.substitutions import AnonName +from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration -from launch.substitutions import NotSubstitution from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode @@ -36,159 +34,31 @@ def load_composable_node_param(param_path): ns = "" pkg = "euclidean_cluster" - # set compare map filter as a component - compare_map_filter_component = ComposableNode( - package="compare_map_segmentation", - namespace=ns, - plugin="compare_map_segmentation::VoxelBasedCompareMapFilterComponent", - name=AnonName("compare_map_filter"), - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("map", LaunchConfiguration("input_map")), - ("output", "map_filter/pointcloud"), - ("map_loader_service", "/map/get_differential_pointcloud_map"), - ("kinematic_state", "/localization/kinematic_state"), - ], - parameters=[load_composable_node_param("compare_map_param_path")], - ) - - # separate range of pointcloud when map_filter used - use_map_short_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", - remappings=[ - ("input", "map_filter/pointcloud"), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - use_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # disuse_map_voxel_grid_filter - disuse_map_short_range_crop_box_filter_component = ComposableNode( + low_height_cropbox_filter_component = ComposableNode( package="pointcloud_preprocessor", namespace=ns, plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="short_distance_crop_box_range", + name="low_height_crop_box_filter", remappings=[ ("input", LaunchConfiguration("input_pointcloud")), - ("output", "short_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": False, - }, - ], - ) - - disuse_map_long_range_crop_box_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::CropBoxFilterComponent", - name="long_distance_crop_box_range", - remappings=[ - ("input", LaunchConfiguration("input_pointcloud")), - ("output", "long_range/pointcloud"), - ], - parameters=[ - load_composable_node_param("voxel_grid_based_euclidean_param_path"), - { - "negative": True, - }, - ], - ) - - # set voxel grid filter as a component - voxel_grid_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::ApproximateDownsampleFilterComponent", - name=AnonName("voxel_grid_filter"), - remappings=[ - ("input", "short_range/pointcloud"), - ("output", "downsampled/short_range/pointcloud"), - ], - parameters=[load_composable_node_param("voxel_grid_param_path")], - ) - - # set outlier filter as a component - outlier_filter_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::VoxelGridOutlierFilterComponent", - name="outlier_filter", - remappings=[ - ("input", "downsampled/short_range/pointcloud"), - ("output", "outlier_filter/pointcloud"), - ], - parameters=[load_composable_node_param("outlier_param_path")], - ) - - # concat with-outlier pointcloud and without-outlier pcl - downsample_concat_component = ComposableNode( - package="pointcloud_preprocessor", - namespace=ns, - plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", - name="concat_downsampled_pcl", - remappings=[("output", "downsampled/concatenated/pointcloud")], - parameters=[ - { - "input_topics": ["long_range/pointcloud", "outlier_filter/pointcloud"], - "output_frame": "base_link", - } - ], - extra_arguments=[{"use_intra_process_comms": True}], - ) - - # set euclidean cluster as a component - use_downsample_euclidean_cluster_component = ComposableNode( - package=pkg, - namespace=ns, - plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", - name="euclidean_cluster", - remappings=[ - ("input", "downsampled/concatenated/pointcloud"), - ("output", LaunchConfiguration("output_clusters")), + ("output", "low_height/pointcloud"), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - use_map_disuse_downsample_euclidean_component = ComposableNode( + use_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", name="euclidean_cluster", remappings=[ - ("input", "map_filter/pointcloud"), + ("input", "low_height/pointcloud"), ("output", LaunchConfiguration("output_clusters")), ], parameters=[load_composable_node_param("voxel_grid_based_euclidean_param_path")], ) - disuse_map_disuse_downsample_euclidean_component = ComposableNode( + + disuse_low_height_euclidean_component = ComposableNode( package=pkg, namespace=ns, plugin="euclidean_cluster::VoxelGridBasedEuclideanClusterNode", @@ -209,75 +79,24 @@ def load_composable_node_param(param_path): output="screen", ) - use_map_use_downsample_loader = LoadComposableNodes( + use_low_height_pointcloud_loader = LoadComposableNodes( composable_node_descriptions=[ - compare_map_filter_component, - use_map_short_range_crop_box_filter_component, - use_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, + low_height_cropbox_filter_component, + use_low_height_euclidean_component, ], target_container=container, - condition=IfCondition( - AndSubstitution( - LaunchConfiguration("use_pointcloud_map"), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), + condition=IfCondition(LaunchConfiguration("use_low_height_cropbox")), ) - disuse_map_use_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_short_range_crop_box_filter_component, - disuse_map_long_range_crop_box_filter_component, - voxel_grid_filter_component, - outlier_filter_component, - downsample_concat_component, - use_downsample_euclidean_cluster_component, - ], + disuse_low_height_pointcloud_loader = LoadComposableNodes( + composable_node_descriptions=[disuse_low_height_euclidean_component], target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - LaunchConfiguration("use_downsample_pointcloud"), - ) - ), - ) - - use_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - compare_map_filter_component, - use_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - (LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), - ) - - disuse_map_disuse_downsample_loader = LoadComposableNodes( - composable_node_descriptions=[ - disuse_map_disuse_downsample_euclidean_component, - ], - target_container=container, - condition=IfCondition( - AndSubstitution( - NotSubstitution(LaunchConfiguration("use_pointcloud_map")), - NotSubstitution(LaunchConfiguration("use_downsample_pointcloud")), - ) - ), + condition=UnlessCondition(LaunchConfiguration("use_low_height_cropbox")), ) return [ container, - use_map_use_downsample_loader, - disuse_map_use_downsample_loader, - use_map_disuse_downsample_loader, - disuse_map_disuse_downsample_loader, + use_low_height_pointcloud_loader, + disuse_low_height_pointcloud_loader, ] @@ -290,29 +109,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("input_pointcloud", "/perception/obstacle_segmentation/pointcloud"), add_launch_arg("input_map", "/map/pointcloud_map"), add_launch_arg("output_clusters", "clusters"), - add_launch_arg("use_pointcloud_map", "false"), - add_launch_arg("use_downsample_pointcloud", "false"), - add_launch_arg( - "voxel_grid_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/voxel_grid.param.yaml", - ], - ), - add_launch_arg( - "outlier_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/outlier.param.yaml", - ], - ), - add_launch_arg( - "compare_map_param_path", - [ - FindPackageShare("autoware_launch"), - "/config/perception/object_recognition/detection/clustering/compare_map.param.yaml", - ], - ), + add_launch_arg("use_low_height_cropbox", "false"), add_launch_arg( "voxel_grid_based_euclidean_param_path", [ diff --git a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml index 2833fed81c..b6a426dabf 100644 --- a/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml +++ b/perception/euclidean_cluster/launch/voxel_grid_based_euclidean_cluster.launch.xml @@ -3,22 +3,14 @@ - - - - - + - - - - - + diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index 53fb4df897..a28791e5d4 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -56,11 +56,19 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( // convert ros to pcl pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); - pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + if (input_msg->data.empty()) { + // NOTE: prevent pcl log spam + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + } else { + pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); + } // clustering std::vector> clusters; - cluster_->cluster(raw_pointcloud_ptr, clusters); + if (!raw_pointcloud_ptr->empty()) { + cluster_->cluster(raw_pointcloud_ptr, clusters); + } // build output msg tier4_perception_msgs::msg::DetectedObjectsWithFeature output; diff --git a/perception/image_projection_based_fusion/CMakeLists.txt b/perception/image_projection_based_fusion/CMakeLists.txt index ce7c3b5ea1..53aafa44b7 100644 --- a/perception/image_projection_based_fusion/CMakeLists.txt +++ b/perception/image_projection_based_fusion/CMakeLists.txt @@ -107,52 +107,8 @@ else() set(CUDNN_AVAIL OFF) endif() -# Create folder to store trained models. -# NOTE: This must be created regardless of CUDA_AVAIL to be specified in ament_auto_package() -set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) -execute_process(COMMAND mkdir -p ${DATA_PATH}) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) - # Download trained models - - message(STATUS "start to download") - function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if("${FILE_HASH}" STREQUAL "${EXISTING_FILE_HASH}") - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} - ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/pointpainting/v4/${FILE_NAME} - ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() - endfunction() - - # default model - download(pts_voxel_encoder_pointpainting.onnx 25c70f76a45a64944ccd19f604c99410) - download(pts_backbone_neck_head_pointpainting.onnx 2c7108245240fbd7bf0104dd68225868) + find_package(OpenCV REQUIRED) find_package(Eigen3 REQUIRED) @@ -197,5 +153,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch config - data ) 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 e9cb76b050..316d29dae1 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 @@ -169,7 +169,8 @@ void RoiClusterFusionNode::fuseOnSingleImage( } for (const auto & feature_obj : input_roi_msg.feature_objects) { - int index = 0; + int index = -1; + bool associated = false; double max_iou = 0.0; bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; @@ -193,8 +194,14 @@ void RoiClusterFusionNode::fuseOnSingleImage( if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { index = cluster_map.first; max_iou = iou + iou_x + iou_y; + associated = true; } } + + if (!associated) { + continue; + } + if (!output_cluster_msg.feature_objects.empty()) { bool is_roi_existence_prob_higher = output_cluster_msg.feature_objects.at(index).object.existence_probability <= diff --git a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt index 24f97e4201..4ae15948ad 100644 --- a/perception/lidar_apollo_instance_segmentation/CMakeLists.txt +++ b/perception/lidar_apollo_instance_segmentation/CMakeLists.txt @@ -24,55 +24,6 @@ if(NOT ${tensorrt_common_FOUND}) endif() find_package(cuda_utils REQUIRED) - -# download weight files -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() - -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD - https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} - STATUS DOWNLOAD_STATUS TIMEOUT 300 - ) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD - https://awf.ml.dev.web.auto/perception/models/lidar_apollo_instance_segmentation/${FILE_NAME} ${FILE_PATH} - STATUS DOWNLOAD_STATUS TIMEOUT 300 - ) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(vlp-16.onnx 63a5a1bb73f7dbb64cf70d04eca45fb4) -download(hdl-64.onnx 009745e33b1df44b68296431cc384cd2) -download(vls-128.onnx b2d709f56f73ae2518c9bf4d0214468f) - add_library(${PROJECT_NAME} SHARED src/node.cpp src/detector.cpp @@ -129,7 +80,6 @@ ament_package() install( DIRECTORY config - data launch DESTINATION share/${PROJECT_NAME} ) diff --git a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp index 53cab74095..1bb6da0374 100644 --- a/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp +++ b/perception/lidar_apollo_instance_segmentation/include/lidar_apollo_instance_segmentation/cluster2d.hpp @@ -79,12 +79,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(MAX_META_TYPE, 0.0); + meta_type_probabilities.assign(MAX_META_TYPE, 0.0); } }; diff --git a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp index a7b1bee69a..8d75853175 100644 --- a/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/cluster2d.cpp @@ -235,13 +235,13 @@ void Cluster2D::classify(const float * inferred_data) for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { int grid = obs->grids[grid_id]; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] += classify_pt_data[k * size_ + grid]; + obs->meta_type_probabilities[k] += classify_pt_data[k * size_ + grid]; } } int meta_type_id = 0; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] /= obs->grids.size(); - if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + obs->meta_type_probabilities[k] /= obs->grids.size(); + if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { meta_type_id = k; } } diff --git a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp index 35655cd16c..4587184ecf 100644 --- a/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp +++ b/perception/lidar_apollo_segmentation_tvm/include/lidar_apollo_segmentation_tvm/cluster2d.hpp @@ -57,12 +57,12 @@ struct Obstacle float height; float heading; MetaType meta_type; - std::vector meta_type_probs; + std::vector meta_type_probabilities; Obstacle() : score(0.0), height(-5.0), heading(0.0), meta_type(MetaType::META_UNKNOWN) { cloud_ptr.reset(new pcl::PointCloud); - meta_type_probs.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); + meta_type_probabilities.assign(static_cast(MetaType::MAX_META_TYPE), 0.0); } }; diff --git a/perception/lidar_apollo_segmentation_tvm/package.xml b/perception/lidar_apollo_segmentation_tvm/package.xml index f4f283abe9..21cd79c27b 100755 --- a/perception/lidar_apollo_segmentation_tvm/package.xml +++ b/perception/lidar_apollo_segmentation_tvm/package.xml @@ -21,6 +21,7 @@ tf2_eigen tf2_geometry_msgs tf2_ros + tier4_autoware_utils tier4_perception_msgs tvm_utility tvm_vendor diff --git a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp index 39c0da04a9..f96a32a5bb 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/cluster2d.cpp @@ -218,13 +218,13 @@ void Cluster2D::classify(const float * inferred_data) for (size_t grid_id = 0; grid_id < obs->grids.size(); grid_id++) { int32_t grid = obs->grids[grid_id]; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] += classify_pt_data[k * siz_ + grid]; + obs->meta_type_probabilities[k] += classify_pt_data[k * siz_ + grid]; } } int meta_type_id = 0; for (int k = 0; k < num_classes; k++) { - obs->meta_type_probs[k] /= static_cast(obs->grids.size()); - if (obs->meta_type_probs[k] > obs->meta_type_probs[meta_type_id]) { + obs->meta_type_probabilities[k] /= static_cast(obs->grids.size()); + if (obs->meta_type_probabilities[k] > obs->meta_type_probabilities[meta_type_id]) { meta_type_id = k; } } diff --git a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp index fb7ad38d7d..905d70235e 100644 --- a/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp +++ b/perception/lidar_apollo_segmentation_tvm/src/lidar_apollo_segmentation_tvm.cpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index f2935ae98a..7ee5622414 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -69,54 +69,7 @@ else() set(CUDNN_AVAIL OFF) endif() -message(STATUS "start to download") if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) -# Download trained models - set(DATA_PATH ${CMAKE_CURRENT_SOURCE_DIR}/data) - set(DOWNLOAD_BASE_URL https://awf.ml.dev.web.auto/perception/models/centerpoint) - execute_process(COMMAND mkdir -p ${DATA_PATH}) - - function(download VERSION FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME} ") - set(DOWNLOAD_URL ${DOWNLOAD_BASE_URL}/${VERSION}/${FILE_NAME}) - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if("${FILE_HASH}" STREQUAL "${EXISTING_FILE_HASH}") - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DOWNLOAD_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DOWNLOAD_URL} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE} ${DOWNLOAD_URL}") - endif() - endfunction() - - # centerpoint - download(v2 pts_voxel_encoder_centerpoint.onnx e8369d7e0f59951cf87c0274830de1fe) - download(v2 pts_backbone_neck_head_centerpoint.onnx 44e44d26781a69c741bf9bddde57fbb3) - - # centerpoint_tiny - download(v2 pts_voxel_encoder_centerpoint_tiny.onnx 5fb774fac44dc83949d05eb3ba077309) - download(v2 pts_backbone_neck_head_centerpoint_tiny.onnx e4658325b70222f7c3637fe00e586b82) - find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -206,7 +159,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_package( INSTALL_TO_SHARE launch - data config ) diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz index 9a390d1089..879837e101 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz @@ -24,8 +24,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel Visualization Manager: @@ -1233,7 +1231,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1820,8 +1818,6 @@ Window Geometry: Height: 1028 Hide Left Dock: true Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz index 8ec93bb5b5..539f89305e 100644 --- a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz @@ -25,8 +25,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel Visualization Manager: @@ -1234,7 +1232,7 @@ Visualization Manager: Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/startplanner + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/start_planner Value: false - Class: rviz_default_plugins/MarkerArray Enabled: false @@ -1821,8 +1819,6 @@ Window Geometry: Height: 1028 Hide Left Dock: true Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 9b01ea8f53..53e19500ff 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -189,7 +189,7 @@ void CenterPointTRT::postProcess(std::vector & det_boxes3d) head_out_heatmap_d_.get(), head_out_offset_d_.get(), head_out_z_d_.get(), head_out_dim_d_.get(), head_out_rot_d_.get(), head_out_vel_d_.get(), det_boxes3d, stream_)); if (det_boxes3d.size() == 0) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("lidar_centerpoint"), "No detected boxes."); } } diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index f694892541..8ef6d628aa 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -17,9 +17,6 @@ #include "map_based_prediction/path_generator.hpp" -#include -#include -#include #include #include #include @@ -33,12 +30,9 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include #include #include diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c1cb9d9dd3..6a414c26b8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -15,7 +15,11 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include +#include +#include +#include #include +#include #include #include #include @@ -26,6 +30,10 @@ #include #include +#include +#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/perception/multi_object_tracker/models.md b/perception/multi_object_tracker/models.md index 8ca12e3191..c83038ffa7 100644 --- a/perception/multi_object_tracker/models.md +++ b/perception/multi_object_tracker/models.md @@ -2,6 +2,8 @@ ## Tracking model + + ### CTRV model [1] CTRV model is a model that assumes constant turn rate and velocity magnitude. @@ -106,5 +108,10 @@ In other words, the offset value must be adjusted so that the input BBOX and the ## References -[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + +[1] Schubert, Robin & Richter, Eric & Wanielik, Gerd. (2008). Comparison and evaluation of advanced motion models for vehicle tracking. 1 - 6. 10.1109/ICIF.2008.4632283. + + + [2] Kong, Jason & Pfeiffer, Mark & Schildbach, Georg & Borrelli, Francesco. (2015). Kinematic and dynamic vehicle models for autonomous driving control design. 1094-1099. 10.1109/IVS.2015.7225830. diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 1e5b9fad9c..6815b59894 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -59,14 +59,16 @@ class ObjectAssociationMergerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; rclcpp::Publisher::SharedPtr merged_object_pub_; - message_filters::Subscriber object0_sub_; - message_filters::Subscriber object1_sub_; - typedef message_filters::sync_policies::ApproximateTime< + message_filters::Subscriber object0_sub_{}; + message_filters::Subscriber object1_sub_{}; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, - autoware_auto_perception_msgs::msg::DetectedObjects> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - Sync sync_; + autoware_auto_perception_msgs::msg::DetectedObjects>; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; + + int sync_queue_size_; std::unique_ptr data_association_; std::string base_link_frame_id_; // associated with the base_link frame diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 3418f7d5a5..5754c256fe 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -6,6 +6,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 0ad2a76f74..f776d2d940 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -77,20 +77,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_), object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), - object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), object0_sub_, object1_sub_) + object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()) { - // Create publishers and subscribers - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); - merged_object_pub_ = create_publisher( - "output/object", rclcpp::QoS{1}); - // Parameters base_link_frame_id_ = declare_parameter("base_link_frame_id", "base_link"); priority_mode_ = static_cast( declare_parameter("priority_mode", static_cast(PriorityMode::Confidence))); + sync_queue_size_ = declare_parameter("sync_queue_size", 20); remove_overlapped_unknown_objects_ = declare_parameter("remove_overlapped_unknown_objects", true); overlapped_judge_param_.precision_threshold = @@ -115,6 +108,16 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); + + // Create publishers and subscribers + using std::placeholders::_1; + using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(sync_queue_size_), object0_sub_, object1_sub_); + sync_ptr_->registerCallback( + std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); + + merged_object_pub_ = create_publisher( + "output/object", rclcpp::QoS{1}); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp index 3b1edca14e..367dcee807 100644 --- a/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp +++ b/perception/probabilistic_occupancy_grid_map/test/test_utils.cpp @@ -24,8 +24,6 @@ // autoware #include "utils/utils.hpp" -#include - // create pointcloud function pcl::PointCloud createPCLPointCloudWithIteratedHeight(const size_t width) { diff --git a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp index c530ee0b2b..947856e5d2 100644 --- a/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp +++ b/perception/radar_crossing_objects_noise_filter/src/radar_crossing_objects_noise_filter_node/radar_crossing_objects_noise_filter_node.cpp @@ -14,7 +14,8 @@ #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/normalization.hpp" #include diff --git a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 839a606c0e..75684d5103 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -296,7 +296,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( bool RadarFusionToDetectedObject::isQualified( const DetectedObject & object, std::shared_ptr> & radars) { - if (object.classification[0].probability > param_.threshold_probability) { + if (object.existence_probability > param_.threshold_probability) { return true; } else { if (!radars || !(*radars).empty()) { diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp index 3fe9172ab5..c43792908a 100644 --- a/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/utils.hpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp index bfd8415970..f6814ceb8c 100644 --- a/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp +++ b/perception/radar_object_tracker/src/tracker/model/linear_motion_tracker.cpp @@ -20,7 +20,8 @@ #include "radar_object_tracker/utils/utils.hpp" -#include +#include +#include #include #include @@ -91,15 +92,15 @@ LinearMotionTracker::LinearMotionTracker( R << cos_yaw, -sin_yaw, sin_yaw, cos_yaw; // covariance matrix in the target vehicle coordinate system - Eigen::Matrix2d P_xy_local, P_vxy_local, P_axy_local; + Eigen::Matrix2d P_xy_local, P_v_xy_local, P_a_xy_local; P_xy_local << ekf_params_.p0_cov_x, 0.0, 0.0, ekf_params_.p0_cov_y; - P_vxy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy; - P_axy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay; + P_v_xy_local << ekf_params_.p0_cov_vx, 0.0, 0.0, ekf_params_.p0_cov_vy; + P_a_xy_local << ekf_params_.p0_cov_ax, 0.0, 0.0, ekf_params_.p0_cov_ay; // Rotated covariance matrix // covariance is rotated by 2D rotation matrix R // P′=R P RT - Eigen::Matrix2d P_xy, P_vxy, P_axy; + Eigen::Matrix2d P_xy, P_v_xy, P_a_xy; // Rotate the covariance matrix according to the vehicle yaw // because p0_cov_x and y are in the vehicle coordinate system. @@ -117,12 +118,12 @@ LinearMotionTracker::LinearMotionTracker( if (object.kinematics.has_twist_covariance) { const auto vx_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X]; const auto vy_cov = object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; - P_vxy_local << vx_cov, 0.0, 0.0, vy_cov; - P_vxy = R * P_vxy_local * R.transpose(); + P_v_xy_local << vx_cov, 0.0, 0.0, vy_cov; + P_v_xy = R * P_v_xy_local * R.transpose(); } else { - P_vxy = R * P_vxy_local * R.transpose(); + P_v_xy = R * P_v_xy_local * R.transpose(); } - // acceleration covariance often writen in object frame + // acceleration covariance often written in object frame const bool has_acceleration_covariance = false; // currently message does not have acceleration covariance if (has_acceleration_covariance) { @@ -132,18 +133,18 @@ LinearMotionTracker::LinearMotionTracker( // const auto ay_cov = // object.kinematics.acceleration_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; // This // is future update - // Eigen::Matrix2d P_axy_local; - // P_axy_local << ax_cov, 0.0, 0.0, ay_cov; - P_axy = R * P_axy_local * R.transpose(); + // Eigen::Matrix2d P_a_xy_local; + // P_a_xy_local << ax_cov, 0.0, 0.0, ay_cov; + P_a_xy = R * P_a_xy_local * R.transpose(); } else { - P_axy = R * P_axy_local * R.transpose(); + P_a_xy = R * P_a_xy_local * R.transpose(); } // put value in P matrix // use block description. This assume x,y,vx,vy,ax,ay in this order P.block<2, 2>(IDX::X, IDX::X) = P_xy; - P.block<2, 2>(IDX::VX, IDX::VX) = P_vxy; - P.block<2, 2>(IDX::AX, IDX::AX) = P_axy; + P.block<2, 2>(IDX::VX, IDX::VX) = P_v_xy; + P.block<2, 2>(IDX::AX, IDX::AX) = P_a_xy; // init shape if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { @@ -292,8 +293,8 @@ bool LinearMotionTracker::predict(const double dt, KalmanFilter & ekf) const // system noise in local coordinate // we assume acceleration random walk model // - // Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T qcov_ax [dt^3/6 0 dt^2/2 0 dt 0] - // + [0 dt^3/6 0 dt^2/2 0 dt] ^ T qcov_ay [0 dt^3/6 0 dt^2/2 0 dt] + // Q_local = [dt^3/6 0 dt^2/2 0 dt 0] ^ T q_cov_ax [dt^3/6 0 dt^2/2 0 dt 0] + // + [0 dt^3/6 0 dt^2/2 0 dt] ^ T q_cov_ay [0 dt^3/6 0 dt^2/2 0 dt] // Eigen::MatrixXd qx = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); // Eigen::MatrixXd qy = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1); // qx << dt * dt * dt / 6, 0, dt * dt / 2, 0, dt, 0; @@ -386,10 +387,10 @@ bool LinearMotionTracker::measureWithPose( // 2. add linear velocity measurement const bool enable_velocity_measurement = object.kinematics.has_twist; if (enable_velocity_measurement) { - Eigen::MatrixXd Cvxvy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); - Cvxvy(0, IDX::VX) = 1; - Cvxvy(1, IDX::VY) = 1; - C_list.push_back(Cvxvy); + Eigen::MatrixXd C_vx_vy = Eigen::MatrixXd::Zero(2, ekf_params_.dim_x); + C_vx_vy(0, IDX::VX) = 1; + C_vx_vy(1, IDX::VY) = 1; + C_list.push_back(C_vx_vy); // velocity is in the target vehicle coordinate system Eigen::MatrixXd Vxy_local = Eigen::MatrixXd::Zero(2, 1); @@ -399,19 +400,19 @@ bool LinearMotionTracker::measureWithPose( Vxy = RotationYaw * Vxy_local; Y_list.push_back(Vxy); - Eigen::Matrix2d Rvxy_local = Eigen::MatrixXd::Zero(2, 2); + Eigen::Matrix2d R_v_xy_local = Eigen::MatrixXd::Zero(2, 2); if (!object.kinematics.has_twist_covariance) { - Rvxy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; + R_v_xy_local << ekf_params_.r_cov_vx, 0, 0, ekf_params_.r_cov_vy; } else { - Rvxy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], 0, - 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; + R_v_xy_local << object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X], + 0, 0, object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::Y_Y]; } - Eigen::MatrixXd Rvxy = Eigen::MatrixXd::Zero(2, 2); - Rvxy = RotationYaw * Rvxy_local * RotationYaw.transpose(); - R_block_list.push_back(Rvxy); + Eigen::MatrixXd R_v_xy = Eigen::MatrixXd::Zero(2, 2); + R_v_xy = RotationYaw * R_v_xy_local * RotationYaw.transpose(); + R_block_list.push_back(R_v_xy); } - // 3. sumup matrices + // 3. sum up matrices const auto row_number = std::accumulate( C_list.begin(), C_list.end(), 0, [](const auto & sum, const auto & C) { return sum + C.rows(); }); @@ -522,9 +523,9 @@ bool LinearMotionTracker::getTrackedObject( auto & twist_with_cov = object.kinematics.twist_with_covariance; auto & acceleration_with_cov = object.kinematics.acceleration_with_covariance; // rotation matrix with yaw_ - Eigen::Matrix2d Ryaw = Eigen::Matrix2d::Zero(); - Ryaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); - const Eigen::Matrix2d Ryaw_inv = Ryaw.transpose(); + Eigen::Matrix2d R_yaw = Eigen::Matrix2d::Zero(); + R_yaw << std::cos(yaw_), -std::sin(yaw_), std::sin(yaw_), std::cos(yaw_); + const Eigen::Matrix2d R_yaw_inv = R_yaw.transpose(); // position pose_with_cov.pose.position.x = X_t(IDX::X); @@ -550,41 +551,41 @@ bool LinearMotionTracker::getTrackedObject( const auto vx = X_t(IDX::VX); const auto vy = X_t(IDX::VY); // rotate this vector with -yaw - Eigen::Vector2d vxy = Ryaw_inv * Eigen::Vector2d(vx, vy); - twist_with_cov.twist.linear.x = vxy(0); - twist_with_cov.twist.linear.y = vxy(1); + Eigen::Vector2d v_local = R_yaw_inv * Eigen::Vector2d(vx, vy); + twist_with_cov.twist.linear.x = v_local(0); + twist_with_cov.twist.linear.y = v_local(1); // acceleration // acceleration need to converted to local coordinate const auto ax = X_t(IDX::AX); const auto ay = X_t(IDX::AY); // rotate this vector with -yaw - Eigen::Vector2d axy = Ryaw_inv * Eigen::Vector2d(ax, ay); - acceleration_with_cov.accel.linear.x = axy(0); - acceleration_with_cov.accel.linear.y = axy(1); + Eigen::Vector2d a_local = R_yaw_inv * Eigen::Vector2d(ax, ay); + acceleration_with_cov.accel.linear.x = a_local(0); + acceleration_with_cov.accel.linear.y = a_local(1); // ===== covariance transformation ===== // since covariance in EKF is in map coordinate and output should be in object coordinate, // we need to transform covariance - Eigen::Matrix2d Pxy_map, Pvxy_map, Paxy_map; - Pxy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); - Pvxy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY); - Paxy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY); + Eigen::Matrix2d P_xy_map, P_v_xy_map, P_a_xy_map; + P_xy_map << P(IDX::X, IDX::X), P(IDX::X, IDX::Y), P(IDX::Y, IDX::X), P(IDX::Y, IDX::Y); + P_v_xy_map << P(IDX::VX, IDX::VX), P(IDX::VX, IDX::VY), P(IDX::VY, IDX::VX), P(IDX::VY, IDX::VY); + P_a_xy_map << P(IDX::AX, IDX::AX), P(IDX::AX, IDX::AY), P(IDX::AY, IDX::AX), P(IDX::AY, IDX::AY); // rotate covariance with -yaw - Eigen::Matrix2d Pxy = Ryaw_inv * Pxy_map * Ryaw_inv.transpose(); - Eigen::Matrix2d Pvxy = Ryaw_inv * Pvxy_map * Ryaw_inv.transpose(); - Eigen::Matrix2d Paxy = Ryaw_inv * Paxy_map * Ryaw_inv.transpose(); + Eigen::Matrix2d P_xy = R_yaw_inv * P_xy_map * R_yaw_inv.transpose(); + Eigen::Matrix2d P_v_xy = R_yaw_inv * P_v_xy_map * R_yaw_inv.transpose(); + Eigen::Matrix2d P_a_xy = R_yaw_inv * P_a_xy_map * R_yaw_inv.transpose(); // position covariance constexpr double no_info_cov = 1e9; // no information constexpr double z_cov = 1. * 1.; // TODO(yukkysaito) Currently tentative constexpr double yaw_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative - pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pxy(IDX::X, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pxy(IDX::X, IDX::Y); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pxy(IDX::Y, IDX::X); - pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pxy(IDX::Y, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_xy(IDX::X, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_xy(IDX::X, IDX::Y); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_xy(IDX::Y, IDX::X); + pose_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_xy(IDX::Y, IDX::Y); pose_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = z_cov; pose_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; pose_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; @@ -594,20 +595,20 @@ bool LinearMotionTracker::getTrackedObject( constexpr double vz_cov = 0.2 * 0.2; // TODO(Yoshi Ri) Currently tentative constexpr double wz_cov = 0.2 * 0.2; // TODO(yukkysaito) Currently tentative - twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Pvxy(IDX::X, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Pvxy(IDX::X, IDX::Y); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Pvxy(IDX::Y, IDX::X); - twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Pvxy(IDX::Y, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_v_xy(IDX::X, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_v_xy(IDX::X, IDX::Y); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_v_xy(IDX::Y, IDX::X); + twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_v_xy(IDX::Y, IDX::Y); twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = wz_cov; // acceleration covariance - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = Paxy(IDX::X, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = Paxy(IDX::X, IDX::Y); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = Paxy(IDX::Y, IDX::X); - acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = Paxy(IDX::Y, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P_a_xy(IDX::X, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::X_Y] = P_a_xy(IDX::X, IDX::Y); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_X] = P_a_xy(IDX::Y, IDX::X); + acceleration_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P_a_xy(IDX::Y, IDX::Y); acceleration_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = no_info_cov; acceleration_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = no_info_cov; acceleration_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = no_info_cov; diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index 75556fbb0c..1122bf7b4b 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -214,8 +214,15 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; kinematics.is_stationary = false; - // Twist conversion - geometry_msgs::msg::Vector3 compensated_velocity = radar_track.velocity; + geometry_msgs::msg::Vector3 compensated_velocity{}; + { + double rotate_yaw = tf2::getYaw(transform_->transform.rotation); + const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; + compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw); + compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw); + compensated_velocity.z = radar_track.velocity.z; + } + if (node_param_.use_twist_compensation) { if (odometry_data_) { compensated_velocity.x += odometry_data_->twist.twist.linear.x; @@ -235,12 +242,12 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() double yaw = tier4_autoware_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); - radar_pose_stamped.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; - + kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); { auto & pose_cov = kinematics.pose_with_covariance.covariance; auto & radar_position_cov = radar_track.position_covariance; diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 73a96da454..693fccb7e9 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -170,7 +170,7 @@ void SimpleObjectMergerNode::onTimer() output_objects.header.frame_id = node_param_.new_frame_id; for (size_t i = 0; i < input_topic_size; i++) { - double time_diff = (this->get_clock()->now()).seconds() - + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); if (std::abs(time_diff) < node_param_.timeout_threshold) { transform_ = transform_listener_->getTransform( diff --git a/perception/tensorrt_yolo/CMakeLists.txt b/perception/tensorrt_yolo/CMakeLists.txt index f97a65834f..b059ab44d8 100755 --- a/perception/tensorrt_yolo/CMakeLists.txt +++ b/perception/tensorrt_yolo/CMakeLists.txt @@ -70,52 +70,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download onnx -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(yolov3.onnx bde3b120f51db251d9a89cd25205e062) -download(yolov4.onnx 99ed8c5d18acad2ebb5ee80860d9cbaf) -download(yolov4-tiny.onnx d16dfd56dddfce75f25d5aaf4c8f4c40) -download(yolov5s.onnx 646b295db6089597e79163446d6eedca) -download(yolov5m.onnx 8fc91d70599ac59ae92c6490001fd84b) -download(yolov5l.onnx 99ff1fc966bda8efbc45c3b8bf00eea2) -download(yolov5x.onnx c51beebed276735cb16a18ca32959e9f) -download(coco.names 8fc50561361f8bcf96b0177086e7616c) - # create calib image directory for int8 calibration set(CALIB_IMAGE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/calib_image") if(NOT EXISTS "${CALIB_IMAGE_PATH}") @@ -186,7 +140,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ament_auto_package(INSTALL_TO_SHARE config - data launch ) @@ -204,7 +157,6 @@ else() message("TensorrtYolo won't be built, CUDA and/or TensorRT were not found.") ament_auto_package(INSTALL_TO_SHARE config - data launch ) endif() diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 01ade9bac4..21860854cc 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -24,49 +24,6 @@ if(OpenMP_FOUND) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") endif() -########## -# Download pretrained model -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 3600) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() - -download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) -download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) -download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) -download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) - ########## # tensorrt_yolox ament_auto_add_library(${PROJECT_NAME} SHARED @@ -157,5 +114,4 @@ endif() ament_auto_package(INSTALL_TO_SHARE launch - data ) diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml index 43f59e944f..cb89f5829c 100644 --- a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -1,8 +1,9 @@ + - + diff --git a/perception/traffic_light_classifier/CMakeLists.txt b/perception/traffic_light_classifier/CMakeLists.txt index 72dba86a00..22bc7521c6 100644 --- a/perception/traffic_light_classifier/CMakeLists.txt +++ b/perception/traffic_light_classifier/CMakeLists.txt @@ -65,59 +65,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# cspell: ignore EFFICIENTNET, MOBILENET -# Download caffemodel and prototxt -set(EFFICIENTNET_BATCH_1_HASH 82baba4fcf1abe0c040cd55005e34510) -set(EFFICIENTNET_BATCH_4_HASH 21b549c2fe4fbb20d32cc019e6d70cd7) -set(EFFICIENTNET_BATCH_6_HASH 28b408710bcb24f4cdd4d746301d4e78) -set(MOBILENET_BATCH_1_HASH caa51f2080aa2df943e4f884c41898eb) -set(MOBILENET_BATCH_4_HASH c2beaf60210f471debfe72b86d076ca0) -set(MOBILENET_BATCH_6_HASH 47255a11bde479320d703f1f45db1242) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) -set(AWS_DIR https://awf.ml.dev.web.auto/perception/models/traffic_light_classifier/v2) -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${AWS_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(traffic_light_classifier_mobilenetv2_batch_1.onnx ${MOBILENET_BATCH_1_HASH}) -download(traffic_light_classifier_mobilenetv2_batch_4.onnx ${MOBILENET_BATCH_4_HASH}) -download(traffic_light_classifier_mobilenetv2_batch_6.onnx ${MOBILENET_BATCH_6_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_1.onnx ${EFFICIENTNET_BATCH_1_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_4.onnx ${EFFICIENTNET_BATCH_4_HASH}) -download(traffic_light_classifier_efficientNet_b1_batch_6.onnx ${EFFICIENTNET_BATCH_6_HASH}) -download(lamp_labels.txt 4b2cf910d97d05d464e7c26901af3d4c) - find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() find_package(OpenCV REQUIRED) @@ -175,7 +122,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) @@ -200,7 +146,6 @@ else() ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_classifier/README.md b/perception/traffic_light_classifier/README.md index 53ba0d3be7..3d15af0cf7 100644 --- a/perception/traffic_light_classifier/README.md +++ b/perception/traffic_light_classifier/README.md @@ -92,67 +92,254 @@ These colors and shapes are assigned to the message as follows: | `red_max_s` | int | the maximum saturation of red color | | `red_max_v` | int | the maximum value (brightness) of red color | -## Customization of CNN model +## Training Traffic Light Classifier Model + +### Overview + +This guide provides detailed instructions on training a traffic light classifier model using the **[mmlab/mmpretrain](https://github.com/open-mmlab/mmpretrain)** repository +and deploying it using **[mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy)**. +If you wish to create a custom traffic light classifier model with your own dataset, please follow the steps outlined below. + +### Data Preparation + +#### Use Sample Dataset + +Autoware offers a sample dataset that illustrates the training procedures for +traffic light classification. This dataset comprises 1045 images categorized +into red, green, and yellow labels. To utilize this sample dataset, +please download it from **[link](https://autoware-files.s3.us-west-2.amazonaws.com/dataset/traffic_light_sample_dataset.tar.gz)** and extract it to a designated folder of your choice. + +#### Use Your Custom Dataset + +To train a traffic light classifier, adopt a structured subfolder format where each +subfolder represents a distinct class. Below is an illustrative dataset structure example; + +```python +DATASET_ROOT + ├── TRAIN + │ ├── RED + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └── ... + │ │ + │ ├── GREEN + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └──... + │ │ + │ ├── YELLOW + │ │ ├── 001.png + │ │ ├── 002.png + │ │ └──... + │ └── ... + │ + ├── VAL + │ └──... + │ + │ + └── TEST + └── ... -Currently, in Autoware, [MobileNetV2](https://arxiv.org/abs/1801.04381v3) and [EfficientNet-b1](https://arxiv.org/abs/1905.11946v5) are provided. -The corresponding onnx files are `data/traffic_light_classifier_mobilenetv2.onnx` and `data/traffic_light_classifier_efficientNet_b1.onnx` (These files will be downloaded during the build process). -Also, you can apply the following models shown as below, for example. -- [ResNet](https://openaccess.thecvf.com/content_cvpr_2016/html/He_Deep_Residual_Learning_CVPR_2016_paper.html) -- [MobileNetV3](https://arxiv.org/abs/1905.02244) - ... +``` + +### Installation + +#### Prerequisites + +**Step 1.** Download and install Miniconda from the [official website](https://mmpretrain.readthedocs.io/en/latest/get_started.html). + +**Step 2.** Create a conda virtual environment and activate it -In order to train models and export onnx model, we recommend [open-mmlab/mmclassification](https://github.com/open-mmlab/mmclassification.git). -Please follow the [official document](https://mmclassification.readthedocs.io/en/latest/) to install and experiment with mmclassification. If you get into troubles, [FAQ page](https://mmclassification.readthedocs.io/en/latest/faq.html) would help you. +```bash +conda create --name tl-classifier python=3.8 -y +conda activate tl-classifier +``` -The following steps are example of a quick-start. +**Step 3.** Install PyTorch -### step 0. Install [MMCV](https://github.com/open-mmlab/mmcv.git) and [MIM](https://github.com/open-mmlab/mim.git) +Please ensure you have PyTorch installed, compatible with CUDA 11.6, as it is a requirement for current Autoware -_NOTE_ : First of all, install [PyTorch](https://pytorch.org/) suitable for your CUDA version (CUDA11.6 is supported in Autoware). +```bash +conda install pytorch==1.13.1 torchvision==0.14.1 pytorch-cuda=11.6 -c pytorch -c nvidia +``` -In order to install mmcv suitable for your CUDA version, install it specifying a url. +#### Install mmlab/mmpretrain -```shell -# Install mim -$ pip install -U openmim +**Step 1.** Install mmpretrain from source -# Install mmcv on a machine with CUDA11.6 and PyTorch1.13.0 -$ pip install mmcv-full -f https://download.openmmlab.com/mmcv/dist/cu116/torch1.13/index.html +```bash +cd ~/ +git clone https://github.com/open-mmlab/mmpretrain.git +cd mmpretrain +pip install -U openmim && mim install -e . ``` -### step 1. Install MMClassification +### Training + +MMPretrain offers a training script that is controlled through a configuration file. +Leveraging an inheritance design pattern, you can effortlessly tailor the training script +using Python files as configuration files. -You can install MMClassification as a Python package or from source. +In the example, we demonstrate the training steps on the MobileNetV2 model, +but you have the flexibility to employ alternative classification models such as +EfficientNetV2, EfficientNetV3, ResNet, and more. -```shell -# As a Python package -$ pip install mmcls +#### Create a config file -# From source -$ git clone https://github.com/open-mmlab/mmclassification.git -$ cd mmclassification -$ pip install -v -e . +Generate a configuration file for your preferred model within the `configs` folder + +```bash +touch ~/mmpretrain/configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py ``` -### step 2. Train your model +Open the configuration file in your preferred text editor and make a copy of +the provided content. Adjust the **data_root** variable to match the path of your dataset. +You are welcome to customize the configuration parameters for the model, dataset, and scheduler to +suit your preferences + +```python +# Inherit model, schedule and default_runtime from base model +_base_ = [ + '../_base_/models/mobilenet_v2_1x.py', + '../_base_/schedules/imagenet_bs256_epochstep.py', + '../_base_/default_runtime.py' +] + +# Set the number of classes to the model +# You can also change other model parameters here +# For detailed descriptions of model parameters, please refer to link below +# (Customize model)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/modules.html] +model = dict(head=dict(num_classes=3, topk=(1, 3))) + +# Set max epochs and validation interval +train_cfg = dict(by_epoch=True, max_epochs=50, val_interval=5) + +# Set optimizer and lr scheduler +optim_wrapper = dict( + optimizer=dict(type='SGD', lr=0.001, momentum=0.9)) +param_scheduler = dict(type='StepLR', by_epoch=True, step_size=1, gamma=0.98) + +dataset_type = 'CustomDataset' +data_root = "/PATH/OF/YOUR/DATASET" + +# Customize data preprocessing and dataloader pipeline for training set +# These parameters calculated for the sample dataset +data_preprocessor = dict( + mean=[0.2888 * 256, 0.2570 * 256, 0.2329 * 256], + std=[0.2106 * 256, 0.2037 * 256, 0.1864 * 256], + num_classes=3, + to_rgb=True, +) + +# Customize data preprocessing and dataloader pipeline for train set +# For detailed descriptions of data pipeline, please refer to link below +# (Customize data pipeline)[https://mmpretrain.readthedocs.io/en/latest/advanced_guides/pipeline.html] +train_pipeline = [ + dict(type='LoadImageFromFile'), + dict(type='Resize', scale=224), + dict(type='RandomFlip', prob=0.5, direction='horizontal'), + dict(type='PackInputs'), +] +train_dataloader = dict( + dataset=dict( + type=dataset_type, + data_root=data_root, + ann_file='', + data_prefix='train', + with_label=True, + pipeline=train_pipeline, + ), + num_workers=8, + batch_size=32, + sampler=dict(type='DefaultSampler', shuffle=True) +) + +# Customize data preprocessing and dataloader pipeline for test set +test_pipeline = [ + dict(type='LoadImageFromFile'), + dict(type='Resize', scale=224), + dict(type='PackInputs'), +] + +# Customize data preprocessing and dataloader pipeline for validation set +val_cfg = dict() +val_dataloader = dict( + dataset=dict( + type=dataset_type, + data_root=data_root, + ann_file='', + data_prefix='val', + with_label=True, + pipeline=test_pipeline, + ), + num_workers=8, + batch_size=32, + sampler=dict(type='DefaultSampler', shuffle=True) +) + +val_evaluator = dict(topk=(1, 3,), type='Accuracy') + +test_dataloader = val_dataloader +test_evaluator = val_evaluator + +``` -Train model with your experiment configuration file. For the details of config file, see [here](https://mmclassification.readthedocs.io/en/latest/tutorials/config.html). +#### Start training -```shell -# [] is optional, you can start training from pre-trained checkpoint -$ mim train mmcls YOUR_CONFIG.py [--resume-from YOUR_CHECKPOINT.pth] +```bash +cd ~/mmpretrain +python tools/train.py configs/mobilenet_v2/mobilenet-v2_8xb32_custom.py ``` -### step 3. Export onnx model +Training logs and weights will be saved in the `work_dirs/mobilenet-v2_8xb32_custom` folder. -In exporting onnx, use `mmclassification/tools/deployment/pytorch2onnx.py` or [open-mmlab/mmdeploy](https://github.com/open-mmlab/mmdeploy.git). +### Convert PyTorh model to ONNX model -```shell -cd ~/mmclassification/tools/deployment -python3 pytorch2onnx.py YOUR_CONFIG.py ... +#### Install mmdeploy + +The 'mmdeploy' toolset is designed for deploying your trained model onto various target devices. +With its capabilities, you can seamlessly convert PyTorch models into the ONNX format. + +```bash +# Activate your conda environment +conda activate tl-classifier + +# Install mmenigne and mmcv +mim install mmengine +mim install "mmcv>=2.0.0rc2" + +# Install mmdeploy +pip install mmdeploy==1.2.0 + +# Support onnxruntime +pip install mmdeploy-runtime==1.2.0 +pip install mmdeploy-runtime-gpu==1.2.0 +pip install onnxruntime-gpu==1.8.1 + +#Clone mmdeploy repository +cd ~/ +git clone -b main https://github.com/open-mmlab/mmdeploy.git ``` +#### Convert PyTorch model to ONNX model + +```bash +cd ~/mmdeploy + +# Run deploy.py script +# deploy.py script takes 5 main arguments with these order; config file path, train config file path, +# checkpoint file path, demo image path, and work directory path +python tools/deploy.py \ +~/mmdeploy/configs/mmpretrain/classification_onnxruntime_static.py\ +~/mmpretrain/configs/mobilenet_v2/train_mobilenet_v2.py \ +~/mmpretrain/work_dirs/train_mobilenet_v2/epoch_300.pth \ +/SAMPLE/IAMGE/DIRECTORY \ +--work-dir mmdeploy_model/mobilenet_v2 +``` + +Converted ONNX model will be saved in the `mmdeploy/mmdeploy_model/mobilenet_v2` folder. + After obtaining your onnx model, update parameters defined in the launch file (e.g. `model_file_path`, `label_file_path`, `input_h`, `input_w`...). Note that, we only support labels defined in [tier4_perception_msgs::msg::TrafficLightElement](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_perception_msgs/msg/traffic_light/TrafficLightElement.msg). diff --git a/perception/traffic_light_fine_detector/CMakeLists.txt b/perception/traffic_light_fine_detector/CMakeLists.txt index a0605a6aa1..f1f30a9da3 100644 --- a/perception/traffic_light_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_fine_detector/CMakeLists.txt @@ -69,53 +69,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download caffemodel and prototxt -set(PRETRAINED_MODEL_BATCH_1_HASH 2b72d085022b8ee6aacff06bd722cfda) -set(PRETRAINED_MODEL_BATCH_4_HASH 4044daa86e7776ce241e94d98a09cc0e) -set(PRETRAINED_MODEL_BATCH_6_HASH 47255a11bde479320d703f1f45db1242) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) -set(DATA_DIR https://awf.ml.dev.web.auto/perception/models/tlr_yolox_s/v2) - -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD ${DATA_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD ${DATA_DIR}/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(tlr_yolox_s_batch_1.onnx ${PRETRAINED_MODEL_BATCH_1_HASH}) -download(tlr_yolox_s_batch_4.onnx ${PRETRAINED_MODEL_BATCH_4_HASH}) -download(tlr_yolox_s_batch_6.onnx ${PRETRAINED_MODEL_BATCH_6_HASH}) -download(tlr_labels.txt ${LAMP_LABEL_HASH}) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) include_directories( ${OpenCV_INCLUDE_DIRS} @@ -147,7 +100,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp index 9db8d7242c..e46431a7b1 100644 --- a/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp +++ b/perception/traffic_light_map_based_detector/include/traffic_light_map_based_detector/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_BASED_DETECTOR__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp index bdfd07ae54..390b0f590b 100644 --- a/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp +++ b/perception/traffic_light_multi_camera_fusion/include/traffic_light_multi_camera_fusion/node.hpp @@ -15,8 +15,6 @@ #ifndef TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ #define TRAFFIC_LIGHT_MULTI_CAMERA_FUSION__NODE_HPP_ -#include -#include #include #include @@ -25,7 +23,7 @@ #include #include -#include +#include #include #include #include diff --git a/perception/traffic_light_multi_camera_fusion/src/node.cpp b/perception/traffic_light_multi_camera_fusion/src/node.cpp index 6c29be9858..1582040648 100644 --- a/perception/traffic_light_multi_camera_fusion/src/node.cpp +++ b/perception/traffic_light_multi_camera_fusion/src/node.cpp @@ -14,6 +14,9 @@ #include "traffic_light_multi_camera_fusion/node.hpp" +#include +#include + #include #include #include diff --git a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp index e37083f81d..7e2825197f 100644 --- a/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp +++ b/perception/traffic_light_occlusion_predictor/include/traffic_light_occlusion_predictor/occlusion_predictor.hpp @@ -25,9 +25,7 @@ #include #include -#include -#include -#include +#include #include #include #include diff --git a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt index 76a0e3d75c..61fa7bcbe0 100644 --- a/perception/traffic_light_ssd_fine_detector/CMakeLists.txt +++ b/perception/traffic_light_ssd_fine_detector/CMakeLists.txt @@ -70,48 +70,6 @@ else() set(CUDNN_AVAIL OFF) endif() -# Download caffemodel and prototxt -set(PRETRAINED_MODEL_HASH 34ce7f2cbacbf6da8bc35769f027b73f) -set(LAMP_LABEL_HASH e9f45efb02f2a9aa8ac27b3d5c164905) - -set(DATA_PATH "${CMAKE_CURRENT_SOURCE_DIR}/data") -if(NOT EXISTS "${DATA_PATH}") - execute_process(COMMAND mkdir -p ${DATA_PATH}) -endif() -function(download FILE_NAME FILE_HASH) - message(STATUS "Checking and downloading ${FILE_NAME}") - set(FILE_PATH ${DATA_PATH}/${FILE_NAME}) - set(STATUS_CODE 0) - message(STATUS "start ${FILE_NAME}") - if(EXISTS ${FILE_PATH}) - message(STATUS "found ${FILE_NAME}") - file(MD5 ${FILE_PATH} EXISTING_FILE_HASH) - if(${FILE_HASH} STREQUAL ${EXISTING_FILE_HASH}) - message(STATUS "same ${FILE_NAME}") - message(STATUS "File already exists.") - else() - message(STATUS "diff ${FILE_NAME}") - message(STATUS "File hash changes. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - else() - message(STATUS "not found ${FILE_NAME}") - message(STATUS "File doesn't exists. Downloading now ...") - file(DOWNLOAD https://awf.ml.dev.web.auto/perception/models/${FILE_NAME} ${FILE_PATH} STATUS DOWNLOAD_STATUS TIMEOUT 300) - list(GET DOWNLOAD_STATUS 0 STATUS_CODE) - list(GET DOWNLOAD_STATUS 1 ERROR_MESSAGE) - endif() - if(${STATUS_CODE} EQUAL 0) - message(STATUS "Download completed successfully!") - else() - message(FATAL_ERROR "Error occurred during download: ${ERROR_MESSAGE}") - endif() -endfunction() -download(mb2-ssd-lite-tlr.onnx 34ce7f2cbacbf6da8bc35769f027b73f) -download(voc_labels_tl.txt e9f45efb02f2a9aa8ac27b3d5c164905) - if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) include_directories( ${OpenCV_INCLUDE_DIRS} @@ -174,7 +132,6 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) ) ament_auto_package(INSTALL_TO_SHARE - data launch ) diff --git a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp index 07cb21e60b..3f83cf6e92 100644 --- a/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp +++ b/perception/traffic_light_visualization/include/traffic_light_map_visualizer/node.hpp @@ -16,7 +16,6 @@ #define TRAFFIC_LIGHT_MAP_VISUALIZER__NODE_HPP_ #include -#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp index a9d107e78a..53e06e47a1 100644 --- a/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_map_visualizer/node.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include diff --git a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp index a3b1a2b16a..2f2d80ba43 100644 --- a/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp +++ b/perception/traffic_light_visualization/src/traffic_light_roi_visualizer/nodelet.cpp @@ -127,13 +127,25 @@ void TrafficLightRoiVisualizerNodelet::imageRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_roi : input_tl_roi_msg->rois) { - createRect(cv_ptr->image, tl_roi, cv::Scalar(0, 255, 0)); + ClassificationResult result; + bool has_correspond_traffic_signal = + getClassificationResult(tl_roi.traffic_light_id, *input_traffic_signals_msg, result); + + if (!has_correspond_traffic_signal) { + // does not have classification result + createRect(cv_ptr->image, tl_roi, cv::Scalar(255, 255, 255)); + } else { + // has classification result + createRect(cv_ptr->image, tl_roi, result); + } } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } @@ -182,7 +194,9 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( { cv_bridge::CvImagePtr cv_ptr; try { - cv_ptr = cv_bridge::toCvCopy(input_image_msg, input_image_msg->encoding); + // try to convert to RGB8 from any input encoding, since createRect() only supports RGB8 based + // bbox drawing + cv_ptr = cv_bridge::toCvCopy(input_image_msg, sensor_msgs::image_encodings::RGB8); for (auto tl_rough_roi : input_tl_rough_roi_msg->rois) { // visualize rough roi createRect(cv_ptr->image, tl_rough_roi, cv::Scalar(0, 255, 0)); @@ -208,7 +222,7 @@ void TrafficLightRoiVisualizerNodelet::imageRoughRoiCallback( } } catch (cv_bridge::Exception & e) { RCLCPP_ERROR( - get_logger(), "Could not convert from '%s' to 'bgr8'.", input_image_msg->encoding.c_str()); + get_logger(), "Could not convert from '%s' to 'rgb8'.", input_image_msg->encoding.c_str()); } image_pub_.publish(cv_ptr->toImageMsg()); } diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index f62e418371..542d4daa0c 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -58,6 +58,12 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/marker_utils/lane_change/debug.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(behavior_path_planner_node PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + target_include_directories(behavior_path_planner_node SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index e77f772601..babbdc60d1 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -5,10 +5,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 2.0 # It must be greater than the state_machine's. + center_line_path_interval: 1.0 # goal search goal_search: - search_priority: "efficient_path" # "efficient_path" or "close_goal" + goal_priority: "minimum_weighted_distance" # "minimum_weighted_distance" or "minimum_longitudinal_distance" + minimum_weighted_distance: + lateral_weight: 40.0 + path_priority: "efficient_path" # "efficient_path" or "close_goal" parking_policy: "left_side" # "left_side" or "right_side" forward_goal_search_length: 20.0 backward_goal_search_length: 20.0 @@ -21,17 +25,19 @@ # occupancy grid map occupancy_grid: - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false + use_occupancy_grid_for_goal_search: true + use_occupancy_grid_for_goal_longitudinal_margin: false + use_occupancy_grid_for_path_collision_check: false occupancy_grid_collision_check_margin: 0.0 theta_size: 360 obstacle_threshold: 60 # object recognition object_recognition: - use_object_recognition: true + use_object_recognition: false object_recognition_collision_check_margin: 0.6 # this should be larger than `surround_check_distance` of surround_obstacle_checker object_recognition_collision_check_max_extra_stopping_margin: 1.0 + th_moving_object_velocity: 1.0 # pull over pull_over: @@ -110,12 +116,13 @@ path_safety_check: # EgoPredictedPath ego_predicted_path: + min_velocity: 0.0 acceleration: 1.0 - time_horizon: 10.0 + max_velocity: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 time_resolution: 0.5 - min_slow_speed: 0.0 delay_until_departure: 1.0 - target_velocity: 1.0 # For target object filtering target_filtering: safety_check_time_horizon: 5.0 @@ -160,6 +167,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon with the value + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 925e70d908..583bdeaaf8 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -55,6 +55,16 @@ motorcycle: true pedestrian: true + # lane change regulations + regulation: + crosswalk: false + intersection: false + + # ego vehicle stuck detection + stuck_detection: + velocity: 0.1 # [m/s] + stop_time: 3.0 # [s] + # collision check enable_prepare_segment_collision_check: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index b62262423f..6760ec787b 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -7,6 +7,8 @@ collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 + th_distance_to_middle_of_the_road: 0.1 + center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true check_shift_path_lane_departure: false @@ -79,12 +81,13 @@ path_safety_check: # EgoPredictedPath ego_predicted_path: + min_velocity: 0.0 acceleration: 1.0 - time_horizon: 10.0 + max_velocity: 1.0 + time_horizon_for_front_object: 10.0 + time_horizon_for_rear_object: 10.0 time_resolution: 0.5 - min_slow_speed: 0.0 delay_until_departure: 1.0 - target_velocity: 1.0 # For target object filtering target_filtering: safety_check_time_horizon: 5.0 @@ -129,6 +132,8 @@ lateral_distance_max_threshold: 1.0 longitudinal_distance_min_threshold: 1.0 longitudinal_velocity_delta_time: 1.0 + # hysteresis factor to expand/shrink polygon + hysteresis_factor_expand_rate: 1.0 # temporary backward_path_length: 30.0 forward_path_length: 100.0 diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md index 4cc8910acf..ee337555ce 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md @@ -118,11 +118,12 @@ Either one is activated when all conditions are met. ## General parameters for goal_planner -| Name | Unit | Type | Description | Default value | -| :------------------ | :---- | :----- | :------------------------------------------------- | :------------ | -| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------ | :---- | :----- | :------------------------------------------------- | :------------ | +| th_arrived_distance | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time | [s] | double | time threshold for arrival of path termination | 2.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## **collision check** @@ -132,13 +133,14 @@ Generate footprints from ego-vehicle path points and determine obstacle collisio #### Parameters for occupancy grid based collision check -| Name | Unit | Type | Description | Default value | -| :----------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | -| use_occupancy_grid | [-] | bool | flag whether to use occupancy grid for collision check | true | -| use_occupancy_grid_for_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | -| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | -| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | -| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------- | :--- | :----- | :-------------------------------------------------------------------------------------------------------------- | :------------ | +| use_occupancy_grid_for_goal_search | [-] | bool | flag whether to use occupancy grid for goal search collision check | true | +| use_occupancy_grid_for_goal_longitudinal_margin | [-] | bool | flag whether to use occupancy grid for keeping longitudinal margin | false | +| use_occupancy_grid_for_path_collision_check | [-] | bool | flag whether to use occupancy grid for collision check | false | +| occupancy_grid_collision_check_margin | [m] | double | margin to calculate ego-vehicle cells from footprint. | 0.0 | +| theta_size | [-] | int | size of theta angle to be considered. angular resolution for collision check will be 2$\pi$ / theta_size [rad]. | 360 | +| obstacle_threshold | [-] | int | threshold of cell values to be considered as obstacles | 60 | ### **object recognition based collision check** @@ -159,18 +161,19 @@ searched for in certain range of the shoulder lane. ### Parameters for goal search -| Name | Unit | Type | Description | Default value | -| :------------------------------ | :--- | :----- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------- | -| search_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | -| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | -| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | -| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | -| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | -| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | -| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | -| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | :-------------------------- | +| goal_priority | [-] | string | In case `minimum_weighted_distance`, sort with smaller longitudinal distances taking precedence over smaller lateral distances. In case `minimum_longitudinal_distance`, sort with weighted lateral distance against longitudinal distance. | `minimum_weighted_distance` | +| path_priority | [-] | string | In case `efficient_path` use a goal that can generate an efficient path( priority is `shift_parking` -> `arc_forward_parking` -> `arc_backward_parking`). In case `close_goal` use the closest goal to the original one. | efficient_path | +| forward_goal_search_length | [m] | double | length of forward range to be explored from the original goal | 20.0 | +| backward_goal_search_length | [m] | double | length of backward range to be explored from the original goal | 20.0 | +| goal_search_interval | [m] | double | distance interval for goal search | 2.0 | +| longitudinal_margin | [m] | double | margin between ego-vehicle at the goal position and obstacles | 3.0 | +| max_lateral_offset | [m] | double | maximum offset of goal search in the lateral direction | 0.5 | +| lateral_offset_interval | [m] | double | distance interval of goal search in the lateral direction | 0.25 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| ignore_distance_from_lane_start | [m] | double | distance from start of pull over lanes for ignoring goal candidates | 0.0 | +| margin_from_boundary | [m] | double | distance margin from edge of the shoulder lane | 0.5 | ## **Pull Over** diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 2fd1ff1cfd..2e5e486afc 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -186,6 +186,14 @@ The following figure illustrates when the lane is blocked in multiple lane chang ![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png) +### Lane change regulations + +If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections. +To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`. +If the ego vehicle gets stuck, to avoid stuck, it enables lane change in crosswalk/intersection. +If the ego vehicle stops more than `stuck_detection.stop_time` seconds, it is regarded as a stuck. +If the ego vehicle velocity is smaller than `stuck_detection.velocity`, it is regarded as stopping. + ### Aborting lane change The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise. @@ -294,6 +302,20 @@ The following parameters are configurable in `lane_change.param.yaml`. | `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true | | `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true | +### Lane change regulations + +| Name | Unit | Type | Description | Default value | +| :------------------------ | ---- | ------- | ------------------------------------- | ------------- | +| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false | +| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false | + +### Ego vehicle stuck detection + +| Name | Unit | Type | Description | Default value | +| :-------------------------- | ----- | ------ | --------------------------------------------------- | ------------- | +| `stuck_detection.velocity` | [m/s] | double | Velocity threshold for ego vehicle stuck detection | 0.1 | +| `stuck_detection.stop_time` | [s] | double | Stop time threshold for ego vehicle stuck detection | 3.0 | + ### Collision checks during lane change The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`. diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md index 2b98775e4f..8322952a0b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design.md @@ -59,16 +59,18 @@ PullOutPath --o PullOutPlannerBase ## General parameters for start_planner -| Name | Unit | Type | Description | Default value | -| :---------------------------------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | -| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | -| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------------- | :---- | :----- | :-------------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_distance_to_middle_of_the_road | [m] | double | distance threshold to determine if the vehicle is on the middle of the road | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_turn_signal_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | +| intersection_search_length | [m] | double | check if intersections exist within this length | 30.0 | +| length_ratio_for_turn_signal_deactivation_near_intersection | [m] | double | deactivate turn signal of this module near intersection | 0.5 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | +| center_line_path_interval | [m] | double | reference center line path point interval | 1.0 | ## Safety check with static obstacles 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 54cfccc494..0f51144c0b 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 @@ -87,6 +87,9 @@ class BehaviorPathPlannerNode : public rclcpp::Node // Getter method for waiting approval modules std::vector getWaitingApprovalModules(); + // Getter method for running modules + std::vector getRunningModules(); + private: rclcpp::Subscription::SharedPtr route_subscriber_; rclcpp::Subscription::SharedPtr vector_map_subscriber_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 78172c6fc0..0a7b59d52f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index d308f55799..100e8046f8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -25,8 +25,7 @@ #include #include -#include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp index 82c387d04e..0dee53d4f6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp @@ -14,7 +14,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/ros/marker_helper.hpp" #include "std_msgs/msg/color_rgba.hpp" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index d337bae17c..1d0c9f5c2c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -17,6 +17,7 @@ #include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" +#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -25,11 +26,16 @@ namespace marker_utils::lane_change_markers { using behavior_path_planner::LaneChangePath; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using visualization_msgs::msg::MarkerArray; MarkerArray showAllValidLaneChangePath( const std::vector & lanes, std::string && ns); MarkerArray createLaneChangingVirtualWallMarker( const geometry_msgs::msg::Pose & lane_changing_pose, const std::string & module_name, const rclcpp::Time & now, const std::string & ns); +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns); } // namespace marker_utils::lane_change_markers #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index c53735486e..86f1a39026 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -18,16 +18,15 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include - #include #include #include #include #include -#include +#include +#include #include #include @@ -41,6 +40,7 @@ using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugPair; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; using geometry_msgs::msg::Pose; @@ -108,6 +108,10 @@ MarkerArray showPolygon(const CollisionCheckDebugMap & obj_debug_vec, std::strin MarkerArray showPredictedPath(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, std::string && ns); + +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id); } // namespace marker_utils #endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 4bdfc807aa..ee5771c982 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -19,14 +19,17 @@ #include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_following/module_data.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include #include +#include "tier4_debug_msgs/msg/float64_stamped.hpp" #include #include +#include + #include #include #include @@ -43,6 +46,8 @@ using tier4_autoware_utils::StopWatch; using tier4_planning_msgs::msg::StopReasonArray; using SceneModulePtr = std::shared_ptr; using SceneModuleManagerPtr = std::shared_ptr; +using DebugPublisher = tier4_autoware_utils::DebugPublisher; +using DebugDoubleMsg = tier4_debug_msgs::msg::Float64Stamped; enum Action { ADD = 0, @@ -279,32 +284,7 @@ class PlannerManager * @param planner data. * @return reference path. */ - BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const - { - const auto & route_handler = data->route_handler; - const auto & pose = data->self_odometry->pose.pose; - const auto p = data->parameters; - - constexpr double extra_margin = 10.0; - const auto backward_length = - std::max(p.backward_path_length, p.backward_path_length + extra_margin); - - const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); - - lanelet::ConstLanelet closest_lane{}; - if (lanelet::utils::query::getClosestLaneletWithConstrains( - lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, - p.ego_nearest_yaw_threshold)) { - return utils::getReferencePath(closest_lane, data); - } - - if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { - return utils::getReferencePath(closest_lane, data); - } - - return {}; // something wrong. - } + BehaviorModuleOutput getReferencePath(const std::shared_ptr & data) const; /** * @brief stop and unregister the module from manager. @@ -445,6 +425,8 @@ class PlannerManager std::vector candidate_module_ptrs_; + std::unique_ptr debug_publisher_ptr_; + mutable rclcpp::Logger logger_; mutable rclcpp::Clock clock_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 096ae61a94..d362a5e758 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -219,6 +219,8 @@ class AvoidanceModule : public SceneModuleInterface */ void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + void insertReturnDeadLine(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** * @brief insert stop point in output path. * @param target path. @@ -263,10 +265,10 @@ class AvoidanceModule : public SceneModuleInterface * @brief fill candidate shift lines. * @param avoidance data. * @param debug data. - * @details in this function, three shift line sets are generated. + * @details in this function, following two shift line arrays are generated. * - unapproved raw shift lines. * - unapproved new shift lines. - * - safe new shift lines. (avoidance path is generated from this shift line set.) + * and check whether the new shift lines are safe or not. */ void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; @@ -311,8 +313,32 @@ class AvoidanceModule : public SceneModuleInterface * @param debug data. * @return processed shift lines. */ - AvoidLineArray calcRawShiftLinesFromObjects( - AvoidancePlanningData & data, DebugData & debug) const; + AvoidOutlines generateAvoidOutline(AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief merge avoid outlines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyMergeProcess(const AvoidOutlines & outlines, DebugData & debug) const; + + /* + * @brief fill gap between two shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyFillGapProcess(const AvoidOutlines & outlines, DebugData & debug) const; + + /* + * @brief generate candidate shift lines. + * @param one-shot shift lines. + * @param path shifter. + * @param debug data. + */ + AvoidLineArray generateCandidateShiftLine( + const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const; /** * @brief clean up raw shift lines. @@ -325,10 +351,15 @@ class AvoidanceModule : public SceneModuleInterface * 3. merge raw shirt lines. * 4. trim unnecessary shift lines. */ - AvoidLineArray applyPreProcessToRawShiftLines( - AvoidLineArray & current_raw_shift_points, DebugData & debug) const; + AvoidLineArray applyPreProcess(const AvoidOutlines & outlines, DebugData & debug) const; - AvoidLineArray getFillGapShiftLines(const AvoidLineArray & shift_lines) const; + /* + * @brief fill gap among shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidLineArray applyFillGapProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief merge negative & positive shift lines. @@ -336,7 +367,28 @@ class AvoidanceModule : public SceneModuleInterface * @param debug data. * @return processed shift lines. */ - AvoidLineArray mergeShiftLines(const AvoidLineArray & raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyMergeProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param current raw shift line. + * @param current registered shift line. + * @param debug data. + */ + AvoidLineArray applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + [[maybe_unused]] DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param shift lines which the return shift is added. + * Pick up the last shift point, which is the most farthest from ego, from the current candidate + * avoidance points and registered points in the shifter. If the last shift length of the point is + * non-zero, add a return-shift to center line from the point. If there is no shift point in + * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to + * center line from ego. + */ + AvoidLineArray addReturnShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract shift lines from total shift lines based on their gradient. @@ -356,7 +408,7 @@ class AvoidanceModule : public SceneModuleInterface * - Change the shift length to the previous one if the deviation is small. * - Remove unnecessary return shift (back to the center line). */ - AvoidLineArray trimShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; + AvoidLineArray applyTrimProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief extract new shift lines based on current shifted path. the module makes a RTC request @@ -364,24 +416,7 @@ class AvoidanceModule : public SceneModuleInterface * @param candidate shift lines. * @return new shift lines. */ - AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines) const; - - /* - * @brief add return shift line from ego position. - * @param shift lines which the return shift is added. - * Pick up the last shift point, which is the most farthest from ego, from the current candidate - * avoidance points and registered points in the shifter. If the last shift length of the point is - * non-zero, add a return-shift to center line from the point. If there is no shift point in - * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to - * center line from ego. - */ - void addReturnShiftLineFromEgo(AvoidLineArray & shift_lines) const; - - /* - * @brief fill gap between two shift lines. - * @param original shift lines. - */ - void fillShiftLineGap(AvoidLineArray & shift_lines) const; + AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; /* * @brief generate total shift line. total shift line has shift length and gradient array. @@ -397,27 +432,21 @@ class AvoidanceModule : public SceneModuleInterface * @param threshold. shift length is quantized by this value. (if it is 0.3[m], the output shift * length is 0.0, 0.3, 0.6...) */ - void quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applyQuantizeProcess(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief trim shift line whose relative longitudinal distance is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const; + void applySmallShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; /* * @brief merge multiple shift lines whose relative gradient is less than threshold. * @param target shift lines. * @param threshold. */ - void trimSimilarGradShiftLine(AvoidLineArray & shift_lines, const double threshold) const; - - /* - * @brief trim invalid shift lines whose gradient it too large to follow. - * @param target shift lines. - */ - void trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const; + void applySimilarGradFilter(AvoidLineArray & shift_lines, const double threshold) const; /** * @brief add new shift line to path shifter if the RTC status is activated. @@ -432,7 +461,8 @@ class AvoidanceModule : public SceneModuleInterface void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; /** - * @brief validate shift lines. + * @brief once generate avoidance path from new shift lines, and calculate lateral offset between + * ego and the path. * @param new shift lines. * @param path shifter. * @return result. if there is huge gap between the ego position and candidate path, return false. @@ -453,7 +483,7 @@ class AvoidanceModule : public SceneModuleInterface // TODO(murooka) freespace during turning in intersection where there is no neighbor lanes // NOTE: Assume that there is no situation where there is an object in the middle lane of more // than two lanes since which way to avoid is not obvious - void generateExtendedDrivableArea(BehaviorModuleOutput & output) const; + void generateExpandDrivableLanes(BehaviorModuleOutput & output) const; /** * @brief fill debug markers. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index fccc270513..b00818f2f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -37,8 +37,8 @@ namespace behavior_path_planner { struct MinMaxValue { - double min_value; - double max_value; + double min_value{0.0}; + double max_value{0.0}; }; struct DynamicAvoidanceParameters @@ -116,12 +116,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface } } - std::string uuid; - geometry_msgs::msg::Pose pose; + std::string uuid{}; + geometry_msgs::msg::Pose pose{}; autoware_auto_perception_msgs::msg::Shape shape; - double vel; - double lat_vel; - bool is_object_on_ego_path; + double vel{0.0}; + double lat_vel{0.0}; + bool is_object_on_ego_path{false}; std::optional latest_time_inside_ego_path{std::nullopt}; std::vector predicted_paths{}; @@ -129,7 +129,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface // Therefore, they has to be initialized as nullopt. std::optional lon_offset_to_avoid{std::nullopt}; std::optional lat_offset_to_avoid{std::nullopt}; - bool is_collision_left; + bool is_collision_left{false}; bool should_be_avoided{false}; PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; @@ -152,8 +152,8 @@ class DynamicAvoidanceModule : public SceneModuleInterface : max_count_(arg_max_count), min_count_(arg_min_count) { } - int max_count_; - int min_count_; + int max_count_{0}; + int min_count_{0}; void initialize() { current_uuids_.clear(); } void updateObject(const std::string & uuid, const DynamicAvoidanceObject & object) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index a6f9565c08..f6b2fa8666 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -164,7 +164,7 @@ class GoalPlannerModule : public SceneModuleInterface mutable std::shared_ptr objects_filtering_params_; mutable std::shared_ptr safety_check_params_; - vehicle_info_util::VehicleInfo vehicle_info_; + vehicle_info_util::VehicleInfo vehicle_info_{}; // planner std::vector> pull_over_planners_; @@ -174,8 +174,8 @@ class GoalPlannerModule : public SceneModuleInterface // goal searcher std::shared_ptr goal_searcher_; std::optional modified_goal_pose_; - Pose refined_goal_pose_; - GoalCandidates goal_candidates_; + Pose refined_goal_pose_{}; + GoalCandidates goal_candidates_{}; // collision detector // need to be shared_ptr to be used in planner and goal searcher @@ -234,17 +234,6 @@ class GoalPlannerModule : public SceneModuleInterface PathWithLaneId generateStopPath(); PathWithLaneId generateFeasibleStopPath(); - /** - * @brief Generate a stop point in the current path based on the vehicle's pose and constraints. - * - * This function generates a stop point in the current path. If no lanes are available or if - * stopping is not feasible due to constraints (maximum deceleration, maximum jerk), no stop point - * is inserted into the path. - * - * @return the modified path with the stop point inserted. If no feasible stop point can be - * determined, returns an empty optional. - */ - std::optional generateStopInsertedCurrentPath(); void keepStoppedWithCurrentPath(PathWithLaneId & path); double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const; @@ -253,7 +242,6 @@ class GoalPlannerModule : public SceneModuleInterface bool isStopped( std::deque & odometry_buffer, const double time); bool hasFinishedCurrentPath(); - bool hasFinishedGoalPlanner(); bool isOnModifiedGoal() const; double calcModuleRequestLength() const; void resetStatus(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 1a7350d82a..a3469dee29 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -154,6 +154,11 @@ class LaneChangeBase return object_debug_after_approval_; } + const LaneChangeTargetObjects & getDebugFilteredObjects() const + { + return debug_filtered_objects_; + } + const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; } const Point & getEgoPosition() const { return getEgoPose().position; } @@ -262,6 +267,7 @@ class LaneChangeBase mutable LaneChangePaths debug_valid_path_{}; mutable CollisionCheckDebugMap object_debug_{}; mutable CollisionCheckDebugMap object_debug_after_approval_{}; + mutable LaneChangeTargetObjects debug_filtered_objects_{}; mutable double object_debug_lifetime_{0.0}; mutable StopWatch stop_watch_; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index ef57c31c28..f1e4483c8b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; - double calcPrepareDuration( + std::vector calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const; @@ -129,6 +129,12 @@ class NormalLaneChange : public LaneChangeBase const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const; + bool hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + + bool hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const; + bool getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, @@ -149,7 +155,12 @@ class NormalLaneChange : public LaneChangeBase void setStopPose(const Pose & stop_pose); + void updateStopTime(); + + double getStopTime() const { return stop_time_; } + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); + double stop_time_{0.0}; }; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index e9a94126d9..d4013fd97b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -28,8 +28,6 @@ #include "behavior_path_planner/utils/utils.hpp" #include -#include -#include #include #include @@ -170,11 +168,12 @@ class StartPlannerModule : public SceneModuleInterface lanelet::ConstLanelets getPathRoadLanes(const PathWithLaneId & path) const; std::vector generateDrivableLanes(const PathWithLaneId & path) const; void updatePullOutStatus(); + void updateStatusAfterBackwardDriving(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, const tier4_autoware_utils::LinearRing2d & vehicle_footprint); bool hasFinishedPullOut() const; - void checkBackFinished(); + bool isBackwardDrivingComplete() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp index b550f132c2..3a983d66eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 215a8bfd97..634ac69bae 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -27,7 +27,8 @@ #include #include -#include +#include +#include #include #include @@ -109,72 +110,66 @@ struct AvoidanceParameters // use intersection area for avoidance bool use_intersection_areas{false}; - // // constrains - // bool use_constraints_for_decel{false}; - - // // policy - // bool use_relaxed_margin_immediately{false}; - // max deceleration for - double max_deceleration; + double max_deceleration{0.0}; // max jerk - double max_jerk; + double max_jerk{0.0}; // comfortable deceleration - double nominal_deceleration; + double nominal_deceleration{0.0}; // comfortable jerk - double nominal_jerk; + double nominal_jerk{0.0}; // To prevent large acceleration while avoidance. - double max_acceleration; + double max_acceleration{0.0}; // upper distance for envelope polygon expansion. - double upper_distance_for_polygon_expansion; + double upper_distance_for_polygon_expansion{0.0}; // lower distance for envelope polygon expansion. - double lower_distance_for_polygon_expansion; + double lower_distance_for_polygon_expansion{0.0}; // Vehicles whose distance to the center of the path is // less than this will not be considered for avoidance. - double threshold_distance_object_is_on_center; + double threshold_distance_object_is_on_center{0.0}; // execute only when there is no intersection behind of the stopped vehicle. - double object_ignore_section_traffic_light_in_front_distance; + double object_ignore_section_traffic_light_in_front_distance{0.0}; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_section_crosswalk_in_front_distance; + double object_ignore_section_crosswalk_in_front_distance{0.0}; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_section_crosswalk_behind_distance; + double object_ignore_section_crosswalk_behind_distance{0.0}; // distance to avoid object detection - double object_check_forward_distance; + double object_check_forward_distance{0.0}; // continue to detect backward vehicles as avoidance targets until they are this distance away - double object_check_backward_distance; + double object_check_backward_distance{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore // the object. - double object_check_goal_distance; + double object_check_goal_distance{0.0}; // use in judge whether the vehicle is parking object on road shoulder - double object_check_shiftable_ratio; + double object_check_shiftable_ratio{0.0}; // minimum road shoulder width. maybe 0.5 [m] - double object_check_min_road_shoulder_width; + double object_check_min_road_shoulder_width{0.0}; // force avoidance - double threshold_time_force_avoidance_for_stopped_vehicle; + double threshold_time_force_avoidance_for_stopped_vehicle{0.0}; // when complete avoidance motion, there is a distance margin with the object // for longitudinal direction - double longitudinal_collision_margin_min_distance; + double longitudinal_collision_margin_min_distance{0.0}; // when complete avoidance motion, there is a time margin with the object // for longitudinal direction - double longitudinal_collision_margin_time; + double longitudinal_collision_margin_time{0.0}; // parameters for safety check area bool enable_safety_check{false}; @@ -193,38 +188,38 @@ struct AvoidanceParameters double safety_check_time_resolution{0.0}; // find adjacent lane vehicles - double safety_check_backward_distance; + double safety_check_backward_distance{0.0}; // transit hysteresis (unsafe to safe) size_t hysteresis_factor_safe_count; - double hysteresis_factor_expand_rate; + double hysteresis_factor_expand_rate{0.0}; // keep target velocity in yield maneuver - double yield_velocity; + double yield_velocity{0.0}; // maximum stop distance - double stop_max_distance; + double stop_max_distance{0.0}; // stop buffer - double stop_buffer; + double stop_buffer{0.0}; // start avoidance after this time to avoid sudden path change - double prepare_time; + double prepare_time{0.0}; // Even if the vehicle speed is zero, avoidance will start after a distance of this much. - double min_prepare_distance; + double min_prepare_distance{0.0}; // minimum slow down speed - double min_slow_down_speed; + double min_slow_down_speed{0.0}; // slow down speed buffer - double buf_slow_down_speed; + double buf_slow_down_speed{0.0}; // nominal avoidance sped - double nominal_avoidance_speed; + double nominal_avoidance_speed{0.0}; // module try to return original path to keep this distance from edge point of the path. - double remain_buffer_distance; + double remain_buffer_distance{0.0}; // The margin is configured so that the generated avoidance trajectory does not come near to the // road shoulder. @@ -235,46 +230,46 @@ struct AvoidanceParameters double hard_road_shoulder_margin{1.0}; // Even if the obstacle is very large, it will not avoid more than this length for right direction - double max_right_shift_length; + double max_right_shift_length{0.0}; // Even if the obstacle is very large, it will not avoid more than this length for left direction - double max_left_shift_length; + double max_left_shift_length{0.0}; // To prevent large acceleration while avoidance. - double max_lateral_acceleration; + double max_lateral_acceleration{0.0}; // For the compensation of the detection lost. Once an object is observed, it is registered and // will be used for planning from the next time. If the object is not observed, it counts up the // lost_count and the registered object will be removed when the count exceeds this max count. - double object_last_seen_threshold; + double object_last_seen_threshold{0.0}; // The avoidance path generation is performed when the shift distance of the // avoidance points is greater than this threshold. // In multiple targets case: if there are multiple vehicles in a row to be avoided, no new // avoidance path will be generated unless their lateral margin difference exceeds this value. - double lateral_execution_threshold; + double lateral_execution_threshold{0.0}; // shift lines whose shift length is less than threshold is added a request with other large shift // line. - double lateral_small_shift_threshold; + double lateral_small_shift_threshold{0.0}; // use for judge if the ego is shifting or not. - double lateral_avoid_check_threshold; + double lateral_avoid_check_threshold{0.0}; // For shift line generation process. The continuous shift length is quantized by this value. - double quantize_filter_threshold; + double quantize_filter_threshold{0.0}; // For shift line generation process. Merge small shift lines. (First step) - double same_grad_filter_1_threshold; + double same_grad_filter_1_threshold{0.0}; // For shift line generation process. Merge small shift lines. (Second step) - double same_grad_filter_2_threshold; + double same_grad_filter_2_threshold{0.0}; // For shift line generation process. Merge small shift lines. (Third step) - double same_grad_filter_3_threshold; + double same_grad_filter_3_threshold{0.0}; // For shift line generation process. Remove sharp(=jerky) shift line. - double sharp_shift_filter_threshold; + double sharp_shift_filter_threshold{0.0}; // policy bool use_shorten_margin_immediately{false}; @@ -301,7 +296,7 @@ struct AvoidanceParameters std::unordered_map object_parameters; // rss parameters - utils::path_safety_checker::RSSparams rss_params; + utils::path_safety_checker::RSSparams rss_params{}; // clip left and right bounds for objects bool enable_bound_clipping{false}; @@ -395,6 +390,9 @@ using ObjectDataArray = std::vector; */ struct AvoidLine : public ShiftLine { + // object side + bool object_on_right = true; + // Distance from ego to start point in Frenet double start_longitudinal = 0.0; @@ -418,6 +416,21 @@ struct AvoidLine : public ShiftLine }; using AvoidLineArray = std::vector; +struct AvoidOutline +{ + AvoidOutline(const AvoidLine & avoid_line, const AvoidLine & return_line) + : avoid_line{avoid_line}, return_line{return_line} + { + } + + AvoidLine avoid_line{}; + + AvoidLine return_line{}; + + AvoidLineArray middle_lines{}; +}; +using AvoidOutlines = std::vector; + /* * avoidance state */ @@ -469,13 +482,13 @@ struct AvoidancePlanningData boost::optional stop_target_object{boost::none}; // raw shift point - AvoidLineArray unapproved_raw_sl{}; + AvoidLineArray raw_shift_line{}; // new shift point - AvoidLineArray unapproved_new_sl{}; + AvoidLineArray new_shift_line{}; // safe shift point - AvoidLineArray safe_new_sl{}; + AvoidLineArray safe_shift_line{}; bool safe{false}; @@ -525,22 +538,27 @@ struct DebugData lanelet::ConstLineStrings3d bounds; - AvoidLineArray current_shift_lines; // in path shifter - AvoidLineArray new_shift_lines; // in path shifter - - AvoidLineArray registered_raw_shift; - AvoidLineArray current_raw_shift; - AvoidLineArray extra_return_shift; - - AvoidLineArray merged; - AvoidLineArray gap_filled; - AvoidLineArray trim_similar_grad_shift; - AvoidLineArray quantized; - AvoidLineArray trim_small_shift; - AvoidLineArray trim_similar_grad_shift_second; - AvoidLineArray trim_similar_grad_shift_third; - AvoidLineArray trim_momentary_return; - AvoidLineArray trim_too_sharp_shift; + // combine process + AvoidLineArray step1_registered_shift_line; + AvoidLineArray step1_current_shift_line; + AvoidLineArray step1_filled_shift_line; + AvoidLineArray step1_merged_shift_line; + AvoidLineArray step1_combined_shift_line; + AvoidLineArray step1_return_shift_line; + AvoidLineArray step1_front_shift_line; + + // create outline process + AvoidLineArray step2_merged_shift_line; + + // trimming process + AvoidLineArray step3_quantize_filtered; + AvoidLineArray step3_noise_filtered; + AvoidLineArray step3_grad_filtered_1st; + AvoidLineArray step3_grad_filtered_2nd; + AvoidLineArray step3_grad_filtered_3rd; + + // registered process + AvoidLineArray step4_new_shift_line; // shift length std::vector pos_shift; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 0dc07f0950..44d5d6f6b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -89,7 +89,8 @@ std::vector generateObstaclePolygonsForDrivableArea( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const std::shared_ptr & parameters); + const bool is_object_front, const bool limit_to_max_velocity, + const std::shared_ptr & parameters); double getLongitudinalVelocity(const Pose & p_ref, const Pose & p_target, const double v); @@ -149,6 +150,11 @@ double extendToRoadShoulderDistanceWithPolygon( void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineArray & lines); +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line); + +void fillAdditionalInfoFromLongitudinal( + const AvoidancePlanningData & data, AvoidOutlines & outlines); + void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines); AvoidLine fillAdditionalInfo(const AvoidancePlanningData & data, const AvoidLine & line); @@ -167,6 +173,10 @@ std::pair separateObjectsByPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, const AvoidancePlanningData & data, const std::shared_ptr & parameters, DebugData & debug); + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); } // namespace behavior_path_planner::utils::avoidance #endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index 9cae5e18f1..c2d36fdd6e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -28,8 +28,7 @@ #include #include -#include -#include +#include #include #include @@ -47,29 +46,28 @@ using geometry_msgs::msg::PoseArray; struct ParallelParkingParameters { - double th_arrived_distance; - double th_stopped_velocity; - double maximum_deceleration; + // common + double center_line_path_interval{0.0}; // forward parking - double after_forward_parking_straight_distance; - double forward_parking_velocity; - double forward_parking_lane_departure_margin; - double forward_parking_path_interval; - double forward_parking_max_steer_angle; + double after_forward_parking_straight_distance{0.0}; + double forward_parking_velocity{0.0}; + double forward_parking_lane_departure_margin{0.0}; + double forward_parking_path_interval{0.0}; + double forward_parking_max_steer_angle{0.0}; // backward parking - double after_backward_parking_straight_distance; - double backward_parking_velocity; - double backward_parking_lane_departure_margin; - double backward_parking_path_interval; - double backward_parking_max_steer_angle; + double after_backward_parking_straight_distance{0.0}; + double backward_parking_velocity{0.0}; + double backward_parking_lane_departure_margin{0.0}; + double backward_parking_path_interval{0.0}; + double backward_parking_max_steer_angle{0.0}; // pull_out - double pull_out_velocity; - double pull_out_lane_departure_margin; - double pull_out_path_interval; - double pull_out_max_steer_angle; + double pull_out_velocity{0.0}; + double pull_out_lane_departure_margin{0.0}; + double pull_out_path_interval{0.0}; + double pull_out_max_steer_angle{0.0}; }; class GeometricParallelParking @@ -78,10 +76,11 @@ class GeometricParallelParking bool isParking() const; bool planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward); + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const bool left_side_parking); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes); + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -108,11 +107,11 @@ class GeometricParallelParking Pose getArcEndPose() const { return arc_end_pose_; } private: - std::shared_ptr planner_data_; - ParallelParkingParameters parameters_; + std::shared_ptr planner_data_{nullptr}; + ParallelParkingParameters parameters_{}; - double R_E_min_; // base_link - double R_Bl_min_; // front_left + double R_E_min_{0.0}; // base_link + double R_Bl_min_{0.0}; // front_left std::vector arc_paths_; std::vector paths_; @@ -121,10 +120,10 @@ class GeometricParallelParking void clearPaths(); std::vector planOneTrial( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - bool is_forward, const double end_pose_offset, const double lane_departure_margin, - const double arc_path_interval); + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); @@ -133,21 +132,23 @@ class GeometricParallelParking const bool is_forward); boost::optional calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const double start_pose_offset, const double R_E_r, const bool is_forward); + const double start_pose_offset, const double R_E_far, const bool is_forward, + const bool left_side_parking); std::vector generatePullOverPaths( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double velocity); + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double velocity); PathWithLaneId generateStraightPath( const Pose & start_pose, const lanelet::ConstLanelets & road_lanes); void setVelocityToArcPaths( std::vector & arc_paths, const double velocity, const bool set_stop_end); // debug - Pose Cr_; - Pose Cl_; - Pose start_pose_; - Pose arc_end_pose_; + Pose Cr_{}; + Pose Cl_{}; + Pose start_pose_{}; + Pose arc_end_pose_{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index c458d017f5..22624bd1e0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -41,79 +41,85 @@ enum class ParkingPolicy { struct GoalPlannerParameters { // general params - double th_arrived_distance; - double th_stopped_velocity; - double th_stopped_time; - double th_blinker_on_lateral_offset; + double th_arrived_distance{0.0}; + double th_stopped_velocity{0.0}; + double th_stopped_time{0.0}; + double th_blinker_on_lateral_offset{0.0}; + double center_line_path_interval{0.0}; // goal search - std::string search_priority; // "efficient_path" or "close_goal" + std::string goal_priority; // "minimum_weighted_distance" or "minimum_longitudinal_distance" + double minimum_weighted_distance_lateral_weight{0.0}; + std::string path_priority; // "efficient_path" or "close_goal" ParkingPolicy parking_policy; // "left_side" or "right_side" - double forward_goal_search_length; - double backward_goal_search_length; - double goal_search_interval; - double longitudinal_margin; - double max_lateral_offset; - double lateral_offset_interval; - double ignore_distance_from_lane_start; - double margin_from_boundary; + + double forward_goal_search_length{0.0}; + double backward_goal_search_length{0.0}; + double goal_search_interval{0.0}; + double longitudinal_margin{0.0}; + double max_lateral_offset{0.0}; + double lateral_offset_interval{0.0}; + double ignore_distance_from_lane_start{0.0}; + double margin_from_boundary{0.0}; // occupancy grid map - bool use_occupancy_grid; - bool use_occupancy_grid_for_longitudinal_margin; - double occupancy_grid_collision_check_margin; - int theta_size; - int obstacle_threshold; + bool use_occupancy_grid_for_goal_search{false}; + bool use_occupancy_grid_for_goal_longitudinal_margin{false}; + bool use_occupancy_grid_for_path_collision_check{false}; + double occupancy_grid_collision_check_margin{0.0}; + int theta_size{0}; + int obstacle_threshold{0}; // object recognition - bool use_object_recognition; - double object_recognition_collision_check_margin; - double object_recognition_collision_check_max_extra_stopping_margin; + bool use_object_recognition{false}; + double object_recognition_collision_check_margin{0.0}; + double object_recognition_collision_check_max_extra_stopping_margin{0.0}; + double th_moving_object_velocity{0.0}; // pull over general params - double pull_over_minimum_request_length; - double pull_over_velocity; - double pull_over_minimum_velocity; - double decide_path_distance; - double maximum_deceleration; - double maximum_jerk; + double pull_over_minimum_request_length{0.0}; + double pull_over_velocity{0.0}; + double pull_over_minimum_velocity{0.0}; + double decide_path_distance{0.0}; + double maximum_deceleration{0.0}; + double maximum_jerk{0.0}; // shift path - bool enable_shift_parking; - int shift_sampling_num; - double maximum_lateral_jerk; - double minimum_lateral_jerk; - double deceleration_interval; - double after_shift_straight_distance; + bool enable_shift_parking{false}; + int shift_sampling_num{0}; + double maximum_lateral_jerk{0.0}; + double minimum_lateral_jerk{0.0}; + double deceleration_interval{0.0}; + double after_shift_straight_distance{0.0}; // parallel parking - bool enable_arc_forward_parking; - bool enable_arc_backward_parking; + bool enable_arc_forward_parking{false}; + bool enable_arc_backward_parking{false}; ParallelParkingParameters parallel_parking_parameters; // freespace parking - bool enable_freespace_parking; + bool enable_freespace_parking{false}; std::string freespace_parking_algorithm; - double freespace_parking_velocity; - double vehicle_shape_margin; - PlannerCommonParam freespace_parking_common_parameters; - AstarParam astar_parameters; - RRTStarParam rrt_star_parameters; + double freespace_parking_velocity{0.0}; + double vehicle_shape_margin{0.0}; + PlannerCommonParam freespace_parking_common_parameters{}; + AstarParam astar_parameters{}; + RRTStarParam rrt_star_parameters{}; // stop condition - double maximum_deceleration_for_stop; - double maximum_jerk_for_stop; + double maximum_deceleration_for_stop{0.0}; + double maximum_jerk_for_stop{0.0}; // hysteresis parameter - double hysteresis_factor_expand_rate; + double hysteresis_factor_expand_rate{0.0}; // path safety checker - utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; - utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; - utils::path_safety_checker::SafetyCheckParams safety_check_params; + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; + utils::path_safety_checker::SafetyCheckParams safety_check_params{}; // debug - bool print_debug_info; + bool print_debug_info{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index bab13a2879..cac1d37e34 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -37,17 +37,6 @@ struct GoalCandidate double lateral_offset{0.0}; size_t id{0}; bool is_safe{true}; - - bool operator<(const GoalCandidate & other) const noexcept - { - const double diff = distance_from_original_goal - other.distance_from_original_goal; - constexpr double eps = 0.01; - if (std::abs(diff) < eps) { - return lateral_offset < other.lateral_offset; - } - - return distance_from_original_goal < other.distance_from_original_goal; - } }; using GoalCandidates = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index 445161f631..dccaed3184 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -57,7 +57,6 @@ class ShiftPullOver : public PullOverPlannerBase LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; - static constexpr double resample_interval_{1.0}; bool left_side_parking_{true}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 0adda77a2a..b7a9248d56 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 0556a78046..27735777c2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -21,6 +21,8 @@ #include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" +#include + #include #include #include @@ -66,22 +68,23 @@ struct LaneChangeParameters bool check_objects_on_other_lanes{true}; bool use_all_predicted_path{false}; - // true by default - bool check_car{true}; // check object car - bool check_truck{true}; // check object truck - bool check_bus{true}; // check object bus - bool check_trailer{true}; // check object trailer - bool check_unknown{true}; // check object unknown - bool check_bicycle{true}; // check object bicycle - bool check_motorcycle{true}; // check object motorbike - bool check_pedestrian{true}; // check object pedestrian + // regulatory elements + bool regulate_on_crosswalk{false}; + bool regulate_on_intersection{false}; + + // ego vehicle stuck detection + double stop_velocity_threshold{0.1}; + double stop_time_threshold{3.0}; + + // true by default for all objects + utils::path_safety_checker::ObjectTypesToCheck object_types_to_check; // safety check - utils::path_safety_checker::RSSparams rss_params; - utils::path_safety_checker::RSSparams rss_params_for_abort; + utils::path_safety_checker::RSSparams rss_params{}; + utils::path_safety_checker::RSSparams rss_params_for_abort{}; // abort - LaneChangeCancelParameters cancel; + LaneChangeCancelParameters cancel{}; double finish_judge_lateral_threshold{0.2}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index f59e0170bd..a4c2c2e695 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -30,7 +30,7 @@ #include #include -#include +#include #include #include @@ -142,8 +142,6 @@ lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const double backward_length); -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters); - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); @@ -160,9 +158,6 @@ std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, const BehaviorPathPlannerParameters & common_parameters, const double resolution); -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution); - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp index 118fb39606..93e4542568 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_following/module_data.hpp @@ -23,11 +23,11 @@ namespace behavior_path_planner struct LaneFollowingParameters { - double lane_change_prepare_duration; + double lane_change_prepare_duration{0.0}; // finding closest lanelet - double distance_threshold; - double yaw_threshold; + double distance_threshold{0.0}; + double yaw_threshold{0.0}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 8a3b7094c6..e1256fd774 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -100,6 +100,7 @@ void filterObjectsByPosition( PredictedObjects & objects, const std::vector & path_points, const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance); + /** * @brief Filters the provided objects based on their classification. * @@ -137,19 +138,31 @@ std::vector getPredictedPathFromObj( const ExtendedPredictedObject & obj, const bool & is_use_all_predicted_path); /** - * @brief Create a predicted path based on ego vehicle parameters. + * @brief Create a predicted path using the provided parameters. * - * @param ego_predicted_path_params Parameters for ego's predicted path. - * @param path_points Points on the path. - * @param vehicle_pose Current pose of the ego-vehicle. + * The function predicts the path based on the current vehicle pose, its current velocity, + * and certain parameters related to the vehicle's behavior and environment. The prediction + * considers acceleration, delay before departure, and maximum velocity constraints. + * + * During the delay before departure, the vehicle's velocity is assumed to be zero, and it does + * not move. After the delay, the vehicle starts to accelerate as per the provided parameters + * until it reaches the maximum allowable velocity or the specified time horizon. + * + * @param ego_predicted_path_params Parameters associated with the ego's predicted path behavior. + * @param path_points Path points to be followed by the vehicle. + * @param vehicle_pose Current pose of the vehicle. * @param current_velocity Current velocity of the vehicle. - * @param ego_seg_idx Index of the ego segment. - * @return std::vector The predicted path. + * @param ego_seg_idx Segment index where the ego vehicle is currently located on the path. + * @param is_object_front Flag indicating if there is an object in front of the ego vehicle. + * @param limit_to_max_velocity Flag indicating if the predicted path should consider the + * maximum allowable velocity. + * @return std::vector Predicted path based on the input parameters. */ std::vector createPredictedPath( const std::shared_ptr & ego_predicted_path_params, const std::vector & path_points, - const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx); + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, + const size_t ego_seg_idx, const bool is_object_front, const bool limit_to_max_velocity); /** * @brief Checks if the centroid of a given object is within the provided lanelets. @@ -187,6 +200,20 @@ TargetObjectsOnLane createTargetObjectsOnLane( const PredictedObjects & filtered_objects, const std::shared_ptr & params); +/** + * @brief Determines whether the predicted object type matches any of the target object types + * specified by the user. + * + * @param object The predicted object whose type is to be checked. + * @param target_object_types A structure containing boolean flags for each object type that the + * user is interested in checking. + * + * @return Returns true if the predicted object's highest probability label matches any of the + * specified target object types. + */ +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types); + } // namespace behavior_path_planner::utils::path_safety_checker #endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 475b38f5bb..889f016e71 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -77,6 +77,7 @@ struct ExtendedPredictedObject autoware_auto_perception_msgs::msg::Shape shape; std::vector predicted_paths; }; +using ExtendedPredictedObjects = std::vector; /** * @brief Specifies which object class should be checked. @@ -128,8 +129,8 @@ struct RSSparams double longitudinal_distance_min_threshold{ 0.0}; ///< Minimum threshold for longitudinal distance. double longitudinal_velocity_delta_time{0.0}; ///< Delta time for longitudinal velocity. - double front_vehicle_deceleration; ///< brake parameter - double rear_vehicle_deceleration; ///< brake parameter + double front_vehicle_deceleration{0.0}; ///< brake parameter + double rear_vehicle_deceleration{0.0}; ///< brake parameter }; /** @@ -137,12 +138,13 @@ struct RSSparams */ struct EgoPredictedPathParams { - double acceleration; ///< Acceleration value. - double time_horizon; ///< Time horizon for prediction. - double time_resolution; ///< Time resolution for prediction. - double min_slow_speed; ///< Minimum slow speed. - double delay_until_departure; ///< Delay before departure. - double target_velocity; ///< Target velocity. + double min_velocity{0.0}; ///< Minimum velocity. + double acceleration{0.0}; ///< Acceleration value. + double max_velocity{0.0}; ///< Maximum velocity. + double time_horizon_for_front_object{0.0}; ///< Time horizon for front object. + double time_horizon_for_rear_object{0.0}; ///< Time horizon for rear object. + double time_resolution{0.0}; ///< Time resolution for prediction. + double delay_until_departure{0.0}; ///< Delay before departure. }; /** @@ -150,18 +152,18 @@ struct EgoPredictedPathParams */ struct ObjectsFilteringParams { - double safety_check_time_horizon; ///< Time horizon for object's prediction. - double safety_check_time_resolution; ///< Time resolution for object's prediction. - double object_check_forward_distance; ///< Forward distance for object checks. - double object_check_backward_distance; ///< Backward distance for object checks. - double ignore_object_velocity_threshold; ///< Velocity threshold for ignoring objects. - ObjectTypesToCheck object_types_to_check; ///< Specifies which object types to check. - ObjectLaneConfiguration object_lane_configuration; ///< Configuration for which lanes to check. - bool include_opposite_lane; ///< Include the opposite lane in checks. - bool invert_opposite_lane; ///< Invert the opposite lane in checks. - bool check_all_predicted_path; ///< Check all predicted paths. - bool use_all_predicted_path; ///< Use all predicted paths. - bool use_predicted_path_outside_lanelet; ///< Use predicted paths outside of lanelets. + double safety_check_time_horizon{0.0}; ///< Time horizon for object's prediction. + double safety_check_time_resolution{0.0}; ///< Time resolution for object's prediction. + double object_check_forward_distance{0.0}; ///< Forward distance for object checks. + double object_check_backward_distance{0.0}; ///< Backward distance for object checks. + double ignore_object_velocity_threshold{0.0}; ///< Velocity threshold for ignoring objects. + ObjectTypesToCheck object_types_to_check{}; ///< Specifies which object types to check. + ObjectLaneConfiguration object_lane_configuration{}; ///< Configuration for which lanes to check. + bool include_opposite_lane{false}; ///< Include the opposite lane in checks. + bool invert_opposite_lane{false}; ///< Invert the opposite lane in checks. + bool check_all_predicted_path{false}; ///< Check all predicted paths. + bool use_all_predicted_path{false}; ///< Use all predicted paths. + bool use_predicted_path_outside_lanelet{false}; ///< Use predicted paths outside of lanelets. }; /** @@ -169,10 +171,12 @@ struct ObjectsFilteringParams */ struct SafetyCheckParams { - bool enable_safety_check; ///< Enable safety checks. - double backward_path_length; ///< Length of the backward lane for path generation. - double forward_path_length; ///< Length of the forward path lane for path generation. - RSSparams rss_params; ///< Parameters related to the RSS model. + bool enable_safety_check{false}; ///< Enable safety checks. + double hysteresis_factor_expand_rate{ + 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. + double backward_path_length{0.0}; ///< Length of the backward lane for path generation. + double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. + RSSparams rss_params{}; ///< Parameters related to the RSS model. bool publish_debug_marker{false}; ///< Option to publish debug markers. }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index a9404038d8..f9fbf70b21 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include +#include #include #include @@ -46,12 +47,16 @@ using autoware_auto_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; +using tier4_autoware_utils::calcYawDeviation; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using vehicle_info_util::VehicleInfo; namespace bg = boost::geometry; +bool isTargetObjectOncoming( + const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose); + bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, const vehicle_info_util::VehicleInfo & vehicle_info); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp index 191cd3e10d..e907df8c7a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp @@ -26,7 +26,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 217b3748fa..37206c7954 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -98,20 +98,10 @@ std::pair getPairsTerminalVelocityAndAccel( const std::vector> & pairs_terminal_velocity_and_accel, const size_t current_path_idx); -/** - * @brief removeInverseOrderPathPoints function - * - * This function is designed to handle a situation that can arise when shifting paths on a curve, - * where the positions of the path points may become inverted (i.e., a point further along the path - * comes to be located before an earlier point). It corrects for this by using the function - * tier4_autoware_utils::isDrivingForward(p1, p2) to check if each pair of adjacent points is in - * the correct order (with the second point being 'forward' of the first). Any points which fail - * this test are omitted from the returned PathWithLaneId. - * - * @param path A path with points possibly in incorrect order. - * @return Returns a new PathWithLaneId that has all points in the correct order. - */ -PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path); +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk); } // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 0842d0a17b..78a64feb2c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -41,9 +41,7 @@ class ShiftPullOut : public PullOutPlannerBase std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, - const behavior_path_planner::StartPlannerParameters & parameter); + const Pose & start_pose, const Pose & goal_pose); double calcBeforeShiftedArcLength( const PathWithLaneId & path, const double target_after_arc_length, const double dr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index 4c095a38e7..6978a7d494 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -35,60 +35,63 @@ using freespace_planning_algorithms::RRTStarParam; struct StartPlannerParameters { - double th_arrived_distance; - double th_stopped_velocity; - double th_stopped_time; - double th_turn_signal_on_lateral_offset; - double intersection_search_length; - double length_ratio_for_turn_signal_deactivation_near_intersection; - double collision_check_margin; - double collision_check_distance_from_end; - double th_moving_object_velocity; + double th_arrived_distance{0.0}; + double th_stopped_velocity{0.0}; + double th_stopped_time{0.0}; + double th_turn_signal_on_lateral_offset{0.0}; + double th_distance_to_middle_of_the_road{0.0}; + double intersection_search_length{0.0}; + double length_ratio_for_turn_signal_deactivation_near_intersection{0.0}; + double collision_check_margin{0.0}; + double collision_check_distance_from_end{0.0}; + double th_moving_object_velocity{0.0}; + double center_line_path_interval{0.0}; + // shift pull out - bool enable_shift_pull_out; - bool check_shift_path_lane_departure; - double minimum_shift_pull_out_distance; - int lateral_acceleration_sampling_num; - double lateral_jerk; - double maximum_lateral_acc; - double minimum_lateral_acc; - double maximum_curvature; // maximum curvature considered in the path generation - double deceleration_interval; + bool enable_shift_pull_out{false}; + bool check_shift_path_lane_departure{false}; + double minimum_shift_pull_out_distance{0.0}; + int lateral_acceleration_sampling_num{0}; + double lateral_jerk{0.0}; + double maximum_lateral_acc{0.0}; + double minimum_lateral_acc{0.0}; + double maximum_curvature{0.0}; // maximum curvature considered in the path generation + double deceleration_interval{0.0}; // geometric pull out - bool enable_geometric_pull_out; - bool divide_pull_out_path; - ParallelParkingParameters parallel_parking_parameters; + bool enable_geometric_pull_out{false}; + bool divide_pull_out_path{false}; + ParallelParkingParameters parallel_parking_parameters{}; // search start pose backward std::string search_priority; // "efficient_path" or "short_back_distance" - bool enable_back; - double backward_velocity; - double max_back_distance; - double backward_search_resolution; - double backward_path_update_duration; - double ignore_distance_from_lane_end; + bool enable_back{false}; + double backward_velocity{0.0}; + double max_back_distance{0.0}; + double backward_search_resolution{0.0}; + double backward_path_update_duration{0.0}; + double ignore_distance_from_lane_end{0.0}; // freespace planner - bool enable_freespace_planner; + bool enable_freespace_planner{false}; std::string freespace_planner_algorithm; - double end_pose_search_start_distance; - double end_pose_search_end_distance; - double end_pose_search_interval; - double freespace_planner_velocity; - double vehicle_shape_margin; + double end_pose_search_start_distance{0.0}; + double end_pose_search_end_distance{0.0}; + double end_pose_search_interval{0.0}; + double freespace_planner_velocity{0.0}; + double vehicle_shape_margin{0.0}; PlannerCommonParam freespace_planner_common_parameters; AstarParam astar_parameters; RRTStarParam rrt_star_parameters; // stop condition - double maximum_deceleration_for_stop; - double maximum_jerk_for_stop; + double maximum_deceleration_for_stop{0.0}; + double maximum_jerk_for_stop{0.0}; // hysteresis parameter - double hysteresis_factor_expand_rate = 0.0; + double hysteresis_factor_expand_rate{0.0}; // path safety checker - utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params; - utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params; - utils::path_safety_checker::SafetyCheckParams safety_check_params; + utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; + utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; + utils::path_safety_checker::SafetyCheckParams safety_check_params{}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index d85e618176..09fb9911f7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index d5e1d22fdc..3d81c584b7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -39,8 +39,7 @@ #include #include -#include -#include +#include #include #ifdef ROS_DISTRO_GALACTIC 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 41df95378c..8302779313 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -66,7 +66,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); - modified_goal_publisher_ = create_publisher("~/output/modified_goal", 1); + const auto durable_qos = rclcpp::QoS(1).transient_local(); + modified_goal_publisher_ = + create_publisher("~/output/modified_goal", durable_qos); stop_reason_publisher_ = create_publisher("~/output/stop_reasons", 1); reroute_availability_publisher_ = create_publisher("~/output/is_reroute_available", 1); @@ -270,6 +272,18 @@ std::vector BehaviorPathPlannerNode::getWaitingApprovalModules() return waiting_approval_modules; } +std::vector BehaviorPathPlannerNode::getRunningModules() +{ + auto all_scene_module_ptr = planner_manager_->getSceneModuleStatus(); + std::vector running_modules; + for (const auto & module : all_scene_module_ptr) { + if (module->status == ModuleStatus::RUNNING) { + running_modules.push_back(module->module_name); + } + } + return running_modules; +} + BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() { BehaviorPathPlannerParameters p{}; diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index b2c326fda7..14a7d5be6a 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -20,6 +20,7 @@ #include +#include #include #include diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index 7493982db7..a1b77b1802 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -98,4 +98,28 @@ MarkerArray createLaneChangingVirtualWallMarker( return marker_array; } +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & current_lane_objects, + const ExtendedPredictedObjects & target_lane_objects, + const ExtendedPredictedObjects & other_lane_objects, const std::string & ns) +{ + int32_t update_id = 0; + auto current_marker = + marker_utils::showFilteredObjects(current_lane_objects, ns, colors::yellow(), update_id); + update_id += static_cast(current_marker.markers.size()); + auto target_marker = + marker_utils::showFilteredObjects(target_lane_objects, ns, colors::aqua(), update_id); + update_id += static_cast(target_marker.markers.size()); + auto other_marker = + marker_utils::showFilteredObjects(other_lane_objects, ns, colors::medium_orchid(), update_id); + + MarkerArray marker_array; + marker_array.markers.insert( + marker_array.markers.end(), current_marker.markers.begin(), current_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), target_marker.markers.begin(), target_marker.markers.end()); + marker_array.markers.insert( + marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end()); + return marker_array; +} } // namespace marker_utils::lane_change_markers diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner/src/marker_utils/utils.cpp index b51fc585ce..a2883403e1 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner/src/marker_utils/utils.cpp @@ -19,9 +19,15 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include #include #include + +#include +#include +#include + +#include + namespace marker_utils { using behavior_path_planner::ShiftLine; @@ -646,4 +652,40 @@ MarkerArray showSafetyCheckInfo(const CollisionCheckDebugMap & obj_debug_vec, st return marker_array; } +MarkerArray showFilteredObjects( + const ExtendedPredictedObjects & predicted_objects, const std::string & ns, + const ColorRGBA & color, int32_t id) +{ + using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; + if (predicted_objects.empty()) { + return MarkerArray{}; + } + + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); + + auto default_cube_marker = + [&](const auto & width, const auto & depth, const auto & color = colors::green()) { + return createDefaultMarker( + "map", now, ns + "_cube", ++id, visualization_msgs::msg::Marker::CUBE, + createMarkerScale(width, depth, 1.0), color); + }; + + MarkerArray marker_array; + marker_array.markers.reserve( + predicted_objects.size()); // poly ego, text ego, poly obj, text obj, cube obj + + std::for_each( + predicted_objects.begin(), predicted_objects.end(), [&](const ExtendedPredictedObject & obj) { + const auto insert_cube_marker = [&](const auto & pose) { + marker_array.markers.emplace_back(); + auto & cube_marker = marker_array.markers.back(); + cube_marker = default_cube_marker(1.0, 1.0, color); + cube_marker.pose = pose; + }; + insert_cube_marker(obj.initial_pose.pose); + }); + + return marker_array; +} + } // namespace marker_utils diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index ee9c445e9f..5963ddb778 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -16,9 +16,10 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" +#include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include +#include #include #include @@ -34,6 +35,7 @@ PlannerManager::PlannerManager(rclcpp::Node & node, const bool verbose) verbose_{verbose} { processing_time_.emplace("total_time", 0.0); + debug_publisher_ptr_ = std::make_unique(&node, "behavior_planner_manager/debug"); } BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) @@ -342,6 +344,34 @@ std::vector PlannerManager::getRequestModules( return request_modules; } +BehaviorModuleOutput PlannerManager::getReferencePath( + const std::shared_ptr & data) const +{ + const auto & route_handler = data->route_handler; + const auto & pose = data->self_odometry->pose.pose; + const auto p = data->parameters; + + constexpr double extra_margin = 10.0; + const auto backward_length = + std::max(p.backward_path_length, p.backward_path_length + extra_margin); + + const auto lanelet_sequence = route_handler->getLaneletSequence( + root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + + lanelet::ConstLanelet closest_lane{}; + if (lanelet::utils::query::getClosestLaneletWithConstrains( + lanelet_sequence, pose, &closest_lane, p.ego_nearest_dist_threshold, + p.ego_nearest_yaw_threshold)) { + return utils::getReferencePath(closest_lane, data); + } + + if (lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane)) { + return utils::getReferencePath(closest_lane, data); + } + + return {}; // something wrong. +} + SceneModulePtr PlannerManager::selectHighestPriorityModule( std::vector & request_modules) const { @@ -856,6 +886,8 @@ void PlannerManager::print() const string_stream << std::right << "[" << std::setw(max_string_num + 1) << std::left << t.first << ":" << std::setw(4) << std::right << t.second << "ms]\n" << std::setw(21); + std::string name = std::string("processing_time/") + t.first; + debug_publisher_ptr_->publish(name, t.second); } RCLCPP_INFO_STREAM(logger_, string_stream.str()); 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 51871b9f6c..83789818c4 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 @@ -32,6 +32,9 @@ #include #include +#include +#include + #include #include #include @@ -66,22 +69,6 @@ using tier4_planning_msgs::msg::AvoidanceDebugFactor; namespace { -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()); -} - bool isDrivingSameLane( const lanelet::ConstLanelets & previous_lanes, const lanelet::ConstLanelets & current_lanes) { @@ -105,6 +92,56 @@ bool isBestEffort(const std::string & policy) { return policy == "best_effort"; } + +AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.start_idx; + ret.start_shift_length = line1.start_shift_length; + ret.start_longitudinal = line1.start_longitudinal; + + ret.end_idx = line2.end_idx; + ret.end_shift_length = line2.end_shift_length; + ret.end_longitudinal = line2.end_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.end_idx; + ret.start_shift_length = line1.end_shift_length; + ret.start_longitudinal = line1.end_longitudinal; + + ret.end_idx = line2.start_idx; + ret.end_shift_length = line2.start_shift_length; + ret.end_longitudinal = line2.start_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLineArray toArray(const AvoidOutlines & outlines) +{ + AvoidLineArray ret{}; + for (const auto & outline : outlines) { + ret.push_back(outline.avoid_line); + ret.push_back(outline.return_line); + + std::for_each( + outline.middle_lines.begin(), outline.middle_lines.end(), + [&ret](const auto & line) { ret.push_back(line); }); + } + return ret; +} } // namespace AvoidanceModule::AvoidanceModule( @@ -129,7 +166,7 @@ bool AvoidanceModule::isExecutionRequested() const return true; } - if (avoid_data_.unapproved_new_sl.empty()) { + if (avoid_data_.new_shift_line.empty()) { return false; } @@ -170,7 +207,10 @@ bool AvoidanceModule::canTransitSuccessState() } } - const bool has_avoidance_target = !data.target_objects.empty(); + const bool has_avoidance_target = + std::any_of(data.target_objects.begin(), data.target_objects.end(), [](const auto & o) { + return o.is_avoidable || o.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + }); const bool has_shift_point = !path_shifter_.getShiftLines().empty(); const bool has_base_offset = std::abs(path_shifter_.getBaseOffset()) > parameters_->lateral_avoid_check_threshold; @@ -385,42 +425,36 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de auto path_shifter = path_shifter_; /** - * STEP 1 - * Create raw shift points from target object. The lateral margin between the ego and the target - * object varies depending on the relative speed between the ego and the target object. + * STEP1: Generate avoid outlines. + * Basically, avoid outlines are generated per target objects. */ - data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data, debug); + const auto outlines = generateAvoidOutline(data, debug); /** - * STEP 2 - * Modify the raw shift points. (Merging, Trimming) + * STEP2: Create rough shift lines. */ - const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + data.raw_shift_line = applyPreProcess(outlines, debug); /** - * STEP 3 - * Find new shift point + * STEP3: Create candidate shift lines. + * Merge rough shift lines, and extract new shift lines. */ - const auto new_sp = findNewShiftLine(processed_raw_sp); - if (isValidShiftLine(new_sp, path_shifter)) { - data.unapproved_new_sl = new_sp; - } - - const auto found_new_sl = data.unapproved_new_sl.size() > 0; + data.new_shift_line = generateCandidateShiftLine(data.raw_shift_line, path_shifter, debug); + const auto found_new_sl = data.new_shift_line.size() > 0; const auto registered = path_shifter.getShiftLines().size() > 0; data.found_avoidance_path = found_new_sl || registered; /** - * STEP 4 - * If there are new shift points, these shift points are registered in path_shifter. + * STEP4: Set new shift lines. + * If there are new shift points, these shift points are registered in path_shifter in order to + * generate candidate avoidance path. */ - if (!data.unapproved_new_sl.empty()) { - addNewShiftLines(path_shifter, data.unapproved_new_sl); + if (!data.new_shift_line.empty()) { + addNewShiftLines(path_shifter, data.new_shift_line); } /** - * STEP 5 - * Generate avoidance path. + * STEP5: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -430,11 +464,11 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP 6 - * Check avoidance path safety. For each target objects and the objects in adjacent lanes, + * STEP6: Check avoidance path safety. + * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.comfortable = isComfortable(data.unapproved_new_sl); + data.comfortable = isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); } @@ -465,7 +499,7 @@ void AvoidanceModule::fillEgoStatus( * If the output path is locked by outside of this module, don't update output path. */ if (isOutputPathLocked()) { - data.safe_new_sl.clear(); + data.safe_shift_line.clear(); data.candidate_path = helper_.getPreviousSplineShiftPath(); RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 500, "this module is locked now. keep current path."); @@ -477,7 +511,7 @@ void AvoidanceModule::fillEgoStatus( */ if (data.safe) { data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; return; } @@ -487,7 +521,7 @@ void AvoidanceModule::fillEgoStatus( */ if (!parameters_->enable_yield_maneuver) { data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; return; } @@ -499,7 +533,7 @@ void AvoidanceModule::fillEgoStatus( if (!can_yield_maneuver) { data.safe = true; // overwrite safety judge. data.yield_required = false; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status."); return; } @@ -509,7 +543,7 @@ void AvoidanceModule::fillEgoStatus( */ { data.yield_required = true; - data.safe_new_sl = data.unapproved_new_sl; + data.safe_shift_line = data.new_shift_line; } /** @@ -531,12 +565,9 @@ void AvoidanceModule::fillEgoStatus( } } -void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const +void AvoidanceModule::fillDebugData( + const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - debug.output_shift = data.candidate_path.shift_length; - debug.current_raw_shift = data.unapproved_raw_sl; - debug.new_shift_lines = data.unapproved_new_sl; - if (!data.stop_target_object) { return; } @@ -545,7 +576,7 @@ void AvoidanceModule::fillDebugData(const AvoidancePlanningData & data, DebugDat return; } - if (data.unapproved_new_sl.empty()) { + if (data.new_shift_line.empty()) { return; } @@ -626,6 +657,8 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif throw std::domain_error("invalid behavior"); } + insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); + setStopReason(StopReason::AVOIDANCE, path.path); } @@ -678,70 +711,85 @@ void AvoidanceModule::updateRegisteredRawShiftLines() printShiftLines(avoid_lines, "registered_raw_shift_lines_ (after)"); registered_raw_shift_lines_ = avoid_lines; - debug_data_.registered_raw_shift = registered_raw_shift_lines_; + debug_data_.step1_registered_shift_line = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( - AvoidLineArray & raw_shift_lines, DebugData & debug) const +AvoidLineArray AvoidanceModule::applyPreProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - const auto fill_gap_shift_lines = getFillGapShiftLines(raw_shift_lines); + AvoidOutlines processed_outlines = outlines; + + /** + * Step1: Rough merge process. + * Merge multiple avoid outlines. If an avoid outlines' return shift line conflicts other + * outline's avoid shift line, those avoid outlines are merged. + */ + processed_outlines = applyMergeProcess(processed_outlines, debug); /** + * Step2: Fill gap process. + * Create and add new shift line to avoid outline in order to fill gaps between avoid shift line + * and middle shift lines, return shift line and middle shift lines. + */ + processed_outlines = applyFillGapProcess(processed_outlines, debug); + + /** + * Step3: Convert to AvoidLineArray from AvoidOutlines. + */ + AvoidLineArray processed_raw_lines = toArray(processed_outlines); + + /** + * Step4: Combine process. * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. - * TODO(Horibe): enrich this logic to be able to consider the removal of the registered - * shift, because it cannot handle the case like "we don't have to avoid - * the object anymore". */ - raw_shift_lines = utils::avoidance::combineRawShiftLinesWithUniqueCheck( - registered_raw_shift_lines_, raw_shift_lines); - - printShiftLines(raw_shift_lines, "raw_shift_lines"); - printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines"); + processed_raw_lines = + applyCombineProcess(processed_raw_lines, registered_raw_shift_lines_, debug); /* + * Step5: Add return shift line. * Add return-to-center shift point from the last shift point, if needed. * If there is no shift points, set return-to center shift from ego. */ - // TODO(Horibe) Here, the return point is calculated considering the prepare distance, - // but there is an issue that sometimes this prepare distance is erased by the trimSimilarGrad, - // and it suddenly tries to return from ego. Then steer rotates aggressively. - // It is temporally solved by changing the threshold of trimSimilarGrad, but it needs to be - // fixed in a proper way. - // Maybe after merge, all shift points before the prepare distance can be deleted. - addReturnShiftLineFromEgo(raw_shift_lines); + processed_raw_lines = addReturnShiftLine(processed_raw_lines, debug); /* - * Add gap filled shift lines so that merged shift lines connect smoothly. + * Step6: Fill gap process. + * Create and add new shift line to avoid lines. */ - fillShiftLineGap(raw_shift_lines); - raw_shift_lines.insert( - raw_shift_lines.end(), fill_gap_shift_lines.begin(), fill_gap_shift_lines.end()); - debug.gap_filled = raw_shift_lines; + return applyFillGapProcess(processed_raw_lines, debug); +} + +AvoidLineArray AvoidanceModule::generateCandidateShiftLine( + const AvoidLineArray & shift_lines, const PathShifter & path_shifter, DebugData & debug) const +{ + AvoidLineArray processed_shift_lines = shift_lines; /** - * On each path point, compute shift length with considering the raw shift points. - * Then create a merged shift points by finding the change point of the gradient of shifting. - * - take maximum shift length if there is duplicate shift point - * - take sum if there are shifts for opposite direction (right and left) - * - shift length is interpolated linearly. - * Note: Because this function just foolishly extracts points, it includes - * insignificant small (useless) shift points, which should be removed in post-process. + * Step1: Merge process. + * Merge positive shift avoid lines and negative shift avoid lines. */ - auto merged_shift_lines = mergeShiftLines(raw_shift_lines, debug); - debug.merged = merged_shift_lines; + processed_shift_lines = applyMergeProcess(processed_shift_lines, debug); - /* - * Remove unnecessary shift points - * - Quantize the shift length to reduce the shift point noise - * - Change the shift length to the previous one if the deviation is small. - * - Combine shift points that have almost same gradient - * - Remove unnecessary return shift (back to the center line). + /** + * Step2: Clean up process. + * Remove noisy shift line and concat same gradient shift lines. + */ + processed_shift_lines = applyTrimProcess(processed_shift_lines, debug); + + /** + * Step3: Extract new shift lines. + * Compare processed shift lines and registered shift lines in order to find new shift lines. */ - auto shift_lines = trimShiftLine(merged_shift_lines, debug); - DEBUG_PRINT("final shift point size = %lu", shift_lines.size()); + processed_shift_lines = findNewShiftLine(processed_shift_lines, debug); - return shift_lines; + /** + * Step4: Validate new shift lines. + * Output new shift lines only when the avoidance path which is generated from them doesn't have + * huge offset from ego. + */ + return isValidShiftLine(processed_shift_lines, path_shifter) ? processed_shift_lines + : AvoidLineArray{}; } void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) @@ -801,7 +849,7 @@ void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) DEBUG_PRINT("registered object size: %lu -> %lu", old_size, registered_raw_shift_lines_.size()); } -AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( +AvoidOutlines AvoidanceModule::generateAvoidOutline( AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { // To be consistent with changes in the ego position, the current shift length is considered. @@ -921,7 +969,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; }; - AvoidLineArray avoid_lines; + AvoidOutlines outlines; for (auto & o : data.target_objects) { if (!o.avoid_margin) { o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; @@ -980,6 +1028,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( al_avoid.end_longitudinal = o.longitudinal - offset; al_avoid.id = getOriginalShiftLineUniqueId(); al_avoid.object = o; + al_avoid.object_on_right = utils::avoidance::isOnRight(o); } AvoidLine al_return; @@ -999,11 +1048,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( o.longitudinal + offset + std::min(feasible_return_distance, return_remaining_distance); al_return.id = getOriginalShiftLineUniqueId(); al_return.object = o; + al_return.object_on_right = utils::avoidance::isOnRight(o); } if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { - avoid_lines.push_back(al_avoid); - avoid_lines.push_back(al_return); + outlines.emplace_back(al_avoid, al_return); } else { o.reason = "InvalidShiftLine"; continue; @@ -1034,9 +1083,11 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects( debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, avoid_lines); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); + + debug.step1_current_shift_line = toArray(outlines); - return avoid_lines; + return outlines; } void AvoidanceModule::generateTotalShiftLine( @@ -1222,98 +1273,184 @@ AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_ return merged_avoid_lines; } -AvoidLineArray AvoidanceModule::getFillGapShiftLines(const AvoidLineArray & shift_lines) const +AvoidOutlines AvoidanceModule::applyMergeProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - AvoidLineArray ret{}; + AvoidOutlines ret{}; - if (shift_lines.empty()) { - return ret; + if (outlines.size() < 2) { + return outlines; } - const auto calc_gap_shift_line = [&](const auto & line1, const auto & line2) { - AvoidLine gap_filled_line{}; - gap_filled_line.start_shift_length = line1.end_shift_length; - gap_filled_line.start_longitudinal = line1.end_longitudinal; - gap_filled_line.end_shift_length = line2.start_shift_length; - gap_filled_line.end_longitudinal = line2.start_longitudinal; - gap_filled_line.id = getOriginalShiftLineUniqueId(); + const auto no_conflict = [](const auto & line1, const auto & line2) { + return line1.end_idx < line2.start_idx || line2.end_idx < line1.start_idx; + }; - return gap_filled_line; + const auto same_side_shift = [](const auto & line1, const auto & line2) { + return line1.object_on_right == line2.object_on_right; }; - // fill gap among shift lines. - for (size_t i = 0; i < shift_lines.size() - 1; i += 2) { - if (shift_lines.at(i).end_longitudinal > shift_lines.at(i + 1).start_longitudinal) { + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + + ret.push_back(outlines.front()); + + for (size_t i = 1; i < outlines.size(); i++) { + auto & last_outline = ret.back(); + auto & next_outline = outlines.at(i); + + const auto & return_line = last_outline.return_line; + const auto & avoid_line = next_outline.avoid_line; + + if (no_conflict(return_line, avoid_line)) { + ret.push_back(outlines.at(i)); + continue; + } + + const auto merged_shift_line = merge(return_line, avoid_line, getOriginalShiftLineUniqueId()); + + if (!isComfortable(AvoidLineArray{merged_shift_line})) { + ret.push_back(outlines.at(i)); + continue; + } + + if (same_side_shift(return_line, avoid_line)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); continue; } - ret.push_back(calc_gap_shift_line(shift_lines.at(i), shift_lines.at(i + 1))); } utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_merged_shift_line); return ret; } -void AvoidanceModule::fillShiftLineGap(AvoidLineArray & shift_lines) const +AvoidOutlines AvoidanceModule::applyFillGapProcess( + const AvoidOutlines & outlines, DebugData & debug) const { - using utils::avoidance::setEndData; + AvoidOutlines ret = outlines; - if (shift_lines.empty()) { - return; - } + for (auto & outline : ret) { + if (outline.middle_lines.empty()) { + const auto new_line = + fill(outline.avoid_line, outline.return_line, getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } - const auto & data = avoid_data_; + helper_.alignShiftLinesOrder(outline.middle_lines, false); - helper_.alignShiftLinesOrder(shift_lines, false); + if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { + const auto new_line = + fill(outline.avoid_line, outline.middle_lines.front(), getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } + + helper_.alignShiftLinesOrder(outline.middle_lines, false); - const auto fill_gap = [&shift_lines, this](const auto & front_line, const auto & back_line) { - const auto has_gap = back_line.start_longitudinal - front_line.end_longitudinal > 0.0; - if (!has_gap) { - return; + if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { + const auto new_line = + fill(outline.middle_lines.back(), outline.return_line, getOriginalShiftLineUniqueId()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); } - AvoidLine new_line{}; - new_line.start_shift_length = front_line.end_shift_length; - new_line.start_longitudinal = front_line.end_longitudinal; - new_line.end_shift_length = back_line.start_shift_length; - new_line.end_longitudinal = back_line.start_longitudinal; - new_line.id = getOriginalShiftLineUniqueId(); + helper_.alignShiftLinesOrder(outline.middle_lines, false); + } - shift_lines.push_back(new_line); - }; + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_filled_shift_line); + + return ret; +} + +AvoidLineArray AvoidanceModule::applyFillGapProcess( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + AvoidLineArray sorted = shift_lines; + + helper_.alignShiftLinesOrder(sorted, false); + + AvoidLineArray ret = sorted; + + if (shift_lines.empty()) { + return ret; + } + + const auto & data = avoid_data_; // fill gap between ego and nearest shift line. - { + if (sorted.front().start_longitudinal > 0.0) { AvoidLine ego_line{}; - setEndData( + utils::avoidance::setEndData( ego_line, helper_.getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); - fill_gap(ego_line, shift_lines.front()); + const auto new_line = fill(ego_line, sorted.front(), getOriginalShiftLineUniqueId()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); } + helper_.alignShiftLinesOrder(sorted, false); + // fill gap among shift lines. - for (size_t i = 0; i < shift_lines.size() - 1; ++i) { - fill_gap(shift_lines.at(i), shift_lines.at(i + 1)); + for (size_t i = 0; i < sorted.size() - 1; ++i) { + if (sorted.at(i + 1).start_longitudinal < sorted.at(i).end_longitudinal) { + continue; + } + + const auto new_line = fill(sorted.at(i), sorted.at(i + 1), getOriginalShiftLineUniqueId()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); } - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, shift_lines); + helper_.alignShiftLinesOrder(ret, false); - helper_.alignShiftLinesOrder(shift_lines, false); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_front_shift_line); + + return ret; } -AvoidLineArray AvoidanceModule::mergeShiftLines( - const AvoidLineArray & raw_shift_lines, DebugData & debug) const +AvoidLineArray AvoidanceModule::applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + DebugData & debug) const { - // Generate shift line by merging raw_shift_lines. + debug.step1_registered_shift_line = registered_lines; + return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); +} + +AvoidLineArray AvoidanceModule::applyMergeProcess( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + // Generate shift line by merging shift_lines. ShiftLineData shift_line_data; - generateTotalShiftLine(raw_shift_lines, shift_line_data); + generateTotalShiftLine(shift_lines, shift_line_data); // Re-generate shift points by detecting gradient-change point of the shift line. auto merged_shift_lines = extractShiftLinesFromLine(shift_line_data); // set parent id for (auto & al : merged_shift_lines) { - al.parent_ids = utils::avoidance::calcParentIds(raw_shift_lines, al); + al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); } // sort by distance from ego. @@ -1328,12 +1465,13 @@ AvoidLineArray AvoidanceModule::mergeShiftLines( debug.neg_shift_grad = shift_line_data.neg_shift_line_grad; debug.total_forward_grad = shift_line_data.forward_grad; debug.total_backward_grad = shift_line_data.backward_grad; + debug.step2_merged_shift_line = merged_shift_lines; } return merged_shift_lines; } -AvoidLineArray AvoidanceModule::trimShiftLine( +AvoidLineArray AvoidanceModule::applyTrimProcess( const AvoidLineArray & shift_lines, DebugData & debug) const { if (shift_lines.empty()) { @@ -1348,65 +1486,51 @@ AvoidLineArray AvoidanceModule::trimShiftLine( // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); } // - Combine avoid points that have almost same gradient. // this is to remove the noise. { const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_1st = sl_array_trimmed; } // - Quantize the shift length to reduce the shift point noise // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. { const auto THRESHOLD = parameters_->quantize_filter_threshold; - quantizeShiftLine(sl_array_trimmed, THRESHOLD); - printShiftLines(sl_array_trimmed, "after sl_array_trimmed"); - debug.quantized = sl_array_trimmed; + applyQuantizeProcess(sl_array_trimmed, THRESHOLD); + debug.step3_quantize_filtered = sl_array_trimmed; } // - Change the shift length to the previous one if the deviation is small. { constexpr double SHIFT_DIFF_THRES = 1.0; - trimSmallShiftLine(sl_array_trimmed, SHIFT_DIFF_THRES); - debug.trim_small_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_small_shift"); + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); + debug.step3_noise_filtered = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_second = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); - } - - // - trimTooSharpShift - // Check if it is not too sharp for the return-to-center shift point. - // If the shift is sharp, it is combined with the next shift point until it gets non-sharp. - { - const auto THRESHOLD = parameters_->sharp_shift_filter_threshold; - trimSharpReturn(sl_array_trimmed, THRESHOLD); - debug.trim_too_sharp_shift = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trimSharpReturn"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_2nd = sl_array_trimmed; } // - Combine avoid points that have almost same gradient (again) { const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; - trimSimilarGradShiftLine(sl_array_trimmed, THRESHOLD); - debug.trim_similar_grad_shift_third = sl_array_trimmed; - printShiftLines(sl_array_trimmed, "after trim_similar_grad_shift_second"); + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_3rd = sl_array_trimmed; } return sl_array_trimmed; } -void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applyQuantizeProcess( + AvoidLineArray & shift_lines, const double threshold) const { if (threshold < 1.0e-5) { return; // no need to process @@ -1419,7 +1543,8 @@ void AvoidanceModule::quantizeShiftLine(AvoidLineArray & shift_lines, const doub helper_.alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::applySmallShiftFilter( + AvoidLineArray & shift_lines, const double threshold) const { if (shift_lines.empty()) { return; @@ -1437,7 +1562,7 @@ void AvoidanceModule::trimSmallShiftLine(AvoidLineArray & shift_lines, const dou } } -void AvoidanceModule::trimSimilarGradShiftLine( +void AvoidanceModule::applySimilarGradFilter( AvoidLineArray & avoid_lines, const double threshold) const { if (avoid_lines.empty()) { @@ -1501,165 +1626,35 @@ void AvoidanceModule::trimSimilarGradShiftLine( DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size()); } -void AvoidanceModule::trimSharpReturn(AvoidLineArray & shift_lines, const double threshold) const +AvoidLineArray AvoidanceModule::addReturnShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const { - AvoidLineArray shift_lines_orig = shift_lines; - shift_lines.clear(); - - const auto isZero = [](double v) { return std::abs(v) < 0.01; }; - - // check if the shift point is positive (avoiding) shift - const auto isPositive = [&](const auto & sl) { - constexpr auto POSITIVE_SHIFT_THR = 0.1; - return std::abs(sl.end_shift_length) - std::abs(sl.start_shift_length) > POSITIVE_SHIFT_THR; - }; - - // check if the shift point is negative (returning) shift - const auto isNegative = [&](const auto & sl) { - constexpr auto NEGATIVE_SHIFT_THR = -0.1; - return std::abs(sl.end_shift_length) - std::abs(sl.start_shift_length) < NEGATIVE_SHIFT_THR; - }; - - // combine two shift points. Be careful the order of "now" and "next". - const auto combineShiftLine = [this](const auto & sl_next, const auto & sl_now) { - auto sl_modified = sl_now; - utils::avoidance::setEndData( - sl_modified, sl_next.end_shift_length, sl_next.end, sl_next.end_idx, - sl_next.end_longitudinal); - sl_modified.parent_ids = - utils::avoidance::concatParentIds(sl_modified.parent_ids, sl_now.parent_ids); - return sl_modified; - }; - - // Check if the merged shift has a conflict with the original shifts. - const auto hasViolation = [&threshold](const auto & combined, const auto & combined_src) { - for (const auto & sl : combined_src) { - const auto combined_shift = - utils::avoidance::lerpShiftLengthOnArc(sl.end_longitudinal, combined); - if (sl.end_shift_length < -0.01 && combined_shift > sl.end_shift_length + threshold) { - return true; - } - if (sl.end_shift_length > 0.01 && combined_shift < sl.end_shift_length - threshold) { - return true; - } - } - return false; - }; - - // check for all shift points - for (size_t i = 0; i < shift_lines_orig.size(); ++i) { - auto sl_now = shift_lines_orig.at(i); - sl_now.start_shift_length = - shift_lines.empty() ? helper_.getEgoLinearShift() : shift_lines.back().end_shift_length; - - if (sl_now.end_shift_length * sl_now.start_shift_length < -0.01) { - DEBUG_PRINT("i = %lu, This is avoid shift for opposite direction. take this one", i); - continue; - } - - // Do nothing for non-reduce shift point - if (!isNegative(sl_now)) { - shift_lines.push_back(sl_now); - DEBUG_PRINT( - "i = %lu, positive shift. take this one. sl_now.length * sl_now.start_length = %f", i, - sl_now.end_shift_length * sl_now.start_shift_length); - continue; - } - - // The last point is out of target of this function. - if (i == shift_lines_orig.size() - 1) { - shift_lines.push_back(sl_now); - DEBUG_PRINT("i = %lu, last shift. take this one.", i); - continue; - } - - // ----------------------------------------------------------------------- - // ------------ From here, the shift point is "negative" ----------------- - // ----------------------------------------------------------------------- - - // if next shift is negative, combine them. loop until combined shift line - // exceeds merged shift point. - DEBUG_PRINT("i = %lu, found negative dist. search.", i); - { - auto sl_combined = sl_now; - auto sl_combined_prev = sl_combined; - AvoidLineArray sl_combined_array{sl_now}; - size_t j = i + 1; - for (; i < shift_lines_orig.size(); ++j) { - const auto sl_combined = combineShiftLine(shift_lines_orig.at(j), sl_now); - - { - std::stringstream ss; - ss << "i = " << i << ", j = " << j << ": sl_combined = " << toStrInfo(sl_combined); - DEBUG_PRINT("%s", ss.str().c_str()); - } - - // it gets positive. Finish merging. - if (isPositive(sl_combined)) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("reach positive."); - break; - } - - // Still negative, but it violates the original shift points. - // Finish with the previous merge result. - if (hasViolation(sl_combined, sl_combined_array)) { - shift_lines.push_back(sl_combined_prev); - DEBUG_PRINT("violation found."); - --j; - break; - } - - // Still negative, but it has an enough long distance. Finish merging. - const auto nominal_distance = - helper_.getMaxAvoidanceDistance(sl_combined.getRelativeLength()); - const auto long_distance = - isZero(sl_combined.end_shift_length) ? nominal_distance : nominal_distance * 5.0; - if (sl_combined.getRelativeLongitudinal() > long_distance) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("still negative, but long enough. Threshold = %f", long_distance); - break; - } - - // It reaches the last point. Still the shift is sharp, but merge with the current result. - if (j == shift_lines_orig.size() - 1) { - shift_lines.push_back(sl_combined); - DEBUG_PRINT("reach end point."); - break; - } - - // Still negative shift, and the distance is not enough. Search next. - sl_combined_prev = sl_combined; - sl_combined_array.push_back(shift_lines_orig.at(j)); - } - i = j; - continue; - } - } - - helper_.alignShiftLinesOrder(shift_lines); - - DEBUG_PRINT("trimSharpReturn: size %lu -> %lu", shift_lines_orig.size(), shift_lines.size()); -} + AvoidLineArray ret = shift_lines; -void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const -{ constexpr double ep = 1.0e-3; const auto & data = avoid_data_; - const bool has_candidate_point = !sl_candidates.empty(); + const bool has_candidate_point = !ret.empty(); const bool has_registered_point = !path_shifter_.getShiftLines().empty(); + const auto exist_unavoidable_object = std::any_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); + + if (exist_unavoidable_object) { + return ret; + } + // If the return-to-center shift points are already registered, do nothing. if (!has_registered_point && std::fabs(getCurrentBaseShift()) < ep) { DEBUG_PRINT("No shift points, not base offset. Do not have to add return-shift."); - return; + return ret; } constexpr double RETURN_SHIFT_THRESHOLD = 0.1; DEBUG_PRINT("registered last shift = %f", path_shifter_.getLastShiftLength()); if (std::abs(path_shifter_.getLastShiftLength()) < RETURN_SHIFT_THRESHOLD) { DEBUG_PRINT("Return shift is already registered. do nothing."); - return; + return ret; } // From here, the return-to-center is not registered. But perhaps the candidate is @@ -1671,8 +1666,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) { // avoidance points: Yes, shift points: No -> select last avoidance point. if (has_candidate_point && !has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - last_sl = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + last_sl = ret.back(); } // avoidance points: No, shift points: Yes -> select last shift point. @@ -1683,8 +1678,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // avoidance points: Yes, shift points: Yes -> select the last one from both. if (has_candidate_point && has_registered_point) { - helper_.alignShiftLinesOrder(sl_candidates, false); - const auto & al = sl_candidates.back(); + helper_.alignShiftLinesOrder(ret, false); + const auto & al = ret.back(); const auto & sl = utils::avoidance::fillAdditionalInfo( data, AvoidLine{path_shifter_.getLastShiftLine().get()}); last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; @@ -1698,7 +1693,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) last_sl.end_shift_length = getCurrentBaseShift(); } } - printShiftLines(ShiftLineArray{last_sl}, "last shift point"); // There already is a shift point candidates to go back to center line, but it could be too sharp // due to detection noise or timing. @@ -1707,7 +1701,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto current_base_shift = helper_.getEgoShift(); if (std::abs(current_base_shift) < ep) { DEBUG_PRINT("last shift almost is zero, and current base_shift is zero. do nothing."); - return; + return ret; } // Is there a shift point in the opposite direction of the current_base_shift? @@ -1715,21 +1709,21 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // the shift length are for return-shift. // Yes -> we can NOT overwrite, because it might be not a return-shift, but a avoiding // shift to the opposite direction which can not be overwritten by the return-shift. - for (const auto & sl : sl_candidates) { + for (const auto & sl : ret) { if ( (current_base_shift > 0.0 && sl.end_shift_length < -ep) || (current_base_shift < 0.0 && sl.end_shift_length > ep)) { DEBUG_PRINT( "try to put overwrite return shift, but there is shift for opposite direction. Skip " "adding return shift."); - return; + return ret; } } // If return shift already exists in candidate or registered shift lines, skip adding return // shift. if (has_candidate_point || has_registered_point) { - return; + return ret; } // set the return-shift from ego. @@ -1746,7 +1740,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); if (arclength_from_ego.empty()) { - return; + return ret; } const auto remaining_distance = arclength_from_ego.back() - parameters_->remain_buffer_distance; @@ -1757,7 +1751,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) // check if there is enough distance for return. if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); - return; + return ret; } // If the remaining distance is not enough, the return shift needs to be shrunk. @@ -1809,9 +1803,8 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = last_sl.end_shift_length; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "prepare for return"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } // shift point for return to center line @@ -1828,12 +1821,11 @@ void AvoidanceModule::addReturnShiftLineFromEgo(AvoidLineArray & sl_candidates) al.end_longitudinal = arclength_from_ego.at(al.end_idx); al.end_shift_length = 0.0; al.start_shift_length = last_sl.end_shift_length; - sl_candidates.push_back(al); - printShiftLines(AvoidLineArray{al}, "return point"); - debug_data_.extra_return_shift.push_back(al); + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); } - DEBUG_PRINT("Return Shift is added."); + return ret; } bool AvoidanceModule::isSafePath( @@ -1845,10 +1837,11 @@ bool AvoidanceModule::isSafePath( return true; // if safety check is disabled, it always return safe. } - const auto ego_predicted_path_for_front_object = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, true, parameters_); - const auto ego_predicted_path_for_rear_object = - utils::avoidance::convertToPredictedPath(shifted_path.path, planner_data_, false, parameters_); + const bool limit_to_max_velocity = false; + const auto ego_predicted_path_for_front_object = utils::avoidance::convertToPredictedPath( + shifted_path.path, planner_data_, true, limit_to_max_velocity, parameters_); + const auto ego_predicted_path_for_rear_object = utils::avoidance::convertToPredictedPath( + shifted_path.path, planner_data_, false, limit_to_max_velocity, parameters_); const auto ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); const auto is_right_shift = [&]() -> std::optional { @@ -1885,13 +1878,13 @@ bool AvoidanceModule::isSafePath( const auto is_object_front = utils::path_safety_checker::isTargetObjectFront(getEgoPose(), obj_polygon, p.vehicle_info); - const auto is_object_incoming = - std::abs(calcYawDeviation(getEgoPose(), object.initial_pose.pose)) > M_PI_2; + const auto is_object_oncoming = + utils::path_safety_checker::isTargetObjectOncoming(getEgoPose(), object.initial_pose.pose); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( object, parameters_->check_all_predicted_path); - const auto & ego_predicted_path = is_object_front && !is_object_incoming + const auto & ego_predicted_path = is_object_front && !is_object_oncoming ? ego_predicted_path_for_front_object : ego_predicted_path_for_rear_object; @@ -1914,148 +1907,12 @@ bool AvoidanceModule::isSafePath( return safe_ || safe_count_ > parameters_->hysteresis_factor_safe_count; } -void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output) const +void AvoidanceModule::generateExpandDrivableLanes(BehaviorModuleOutput & output) const { - const auto has_same_lane = - [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) { - if (lanes.empty()) return false; - const auto has_same = [&](const auto & ll) { return ll.id() == lane.id(); }; - return std::find_if(lanes.begin(), lanes.end(), has_same) != lanes.end(); - }; - - const auto & route_handler = planner_data_->route_handler; - const auto & current_lanes = avoid_data_.current_lanelets; - const auto & enable_opposite = parameters_->use_opposite_lane; std::vector drivable_lanes; - - for (const auto & current_lane : current_lanes) { - DrivableLanes current_drivable_lanes; - current_drivable_lanes.left_lane = current_lane; - current_drivable_lanes.right_lane = current_lane; - - if (!parameters_->use_adjacent_lane) { - drivable_lanes.push_back(current_drivable_lanes); - continue; - } - - // 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)); - } - }; - 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)); - } - }; - - update_left_lanelets(current_lane); - update_right_lanelets(current_lane); - - // 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) { - // get previous lane, and return false if previous lane does not exist - lanelet::ConstLanelets prev_lanes; - if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { - return lanelet::ConstLanelets{}; - } - - lanelet::ConstLanelets next_lanes; - for (const auto & prev_lane : prev_lanes) { - const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); - pushUniqueVector(next_lanes, next_lanes_from_prev); - } - return next_lanes; - }; - - 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 neighbor 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; - } - 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); + for (const auto & lanelet : avoid_data_.current_lanelets) { + drivable_lanes.push_back( + utils::avoidance::generateExpandDrivableLanes(lanelet, planner_data_, parameters_)); } { // for new architecture @@ -2080,14 +1937,17 @@ void AvoidanceModule::generateExtendedDrivableArea(BehaviorModuleOutput & output PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { - // special for avoidance: take behind distance upt ot shift-start-point if it exist. + const auto previous_path = helper_.getPreviousReferencePath(); + const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; for (const auto & pnt : path_shifter_.getShiftLines()) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), pnt.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); } for (const auto & sp : registered_raw_shift_lines_) { - max_dist = std::max(max_dist, calcDistance2d(getEgoPose(), sp.start)); + max_dist = std::max( + max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); } return max_dist; }(); @@ -2095,11 +1955,11 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig const auto extra_margin = 10.0; // Since distance does not consider arclength, but just line. const auto backward_length = std::max( planner_data_->parameters.backward_path_length, longest_dist_to_shift_point + extra_margin); - const auto previous_path = helper_.getPreviousReferencePath(); const size_t orig_ego_idx = planner_data_->findEgoIndex(original_path.points); - const size_t prev_ego_idx = - findNearestSegmentIndex(previous_path.points, getPoint(original_path.points.at(orig_ego_idx))); + const size_t prev_ego_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + previous_path.points, getPose(original_path.points.at(orig_ego_idx)), + std::numeric_limits::max(), planner_data_->parameters.ego_nearest_yaw_threshold); size_t clip_idx = 0; for (size_t i = 0; i < prev_ego_idx; ++i) { @@ -2138,7 +1998,7 @@ BehaviorModuleOutput AvoidanceModule::plan() resetPathCandidate(); resetPathReference(); - updatePathShifter(data.safe_new_sl); + updatePathShifter(data.safe_shift_line); if (data.yield_required) { removeRegisteredShiftLines(); @@ -2208,7 +2068,7 @@ BehaviorModuleOutput AvoidanceModule::plan() utils::clipPathLength(*output.path, ego_idx, planner_data_->parameters); // Drivable area generation. - generateExtendedDrivableArea(output); + generateExpandDrivableLanes(output); setDrivableLanes(output.drivable_area_info.drivable_lanes); return output; @@ -2222,37 +2082,35 @@ CandidateOutput AvoidanceModule::planCandidate() const auto shifted_path = data.candidate_path; - if (!data.safe_new_sl.empty()) { // clip from shift start index for visualize - utils::clipPathLength( - shifted_path.path, data.safe_new_sl.front().start_idx, std::numeric_limits::max(), - 0.0); + if (data.safe_shift_line.empty()) { + const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); + utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); - const auto sl = helper_.getMainShiftLine(data.safe_new_sl); - const auto sl_front = data.safe_new_sl.front(); - const auto sl_back = data.safe_new_sl.back(); + output.path_candidate = shifted_path.path; + return output; + } - output.lateral_shift = helper_.getRelativeShiftToPath(sl); - output.start_distance_to_path_change = sl_front.start_longitudinal; - output.finish_distance_to_path_change = sl_back.end_longitudinal; + const auto sl = helper_.getMainShiftLine(data.safe_shift_line); + const auto sl_front = data.safe_shift_line.front(); + const auto sl_back = data.safe_shift_line.back(); - const uint16_t steering_factor_direction = std::invoke([&output]() { - if (output.lateral_shift > 0.0) { - return SteeringFactor::LEFT; - } - return SteeringFactor::RIGHT; - }); - steering_factor_interface_ptr_->updateSteeringFactor( - {sl_front.start, sl_back.end}, - {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, - ""); - } + utils::clipPathLength( + shifted_path.path, sl_front.start_idx, std::numeric_limits::max(), 0.0); - const size_t ego_idx = planner_data_->findEgoIndex(shifted_path.path.points); - utils::clipPathLength(shifted_path.path, ego_idx, planner_data_->parameters); + output.lateral_shift = helper_.getRelativeShiftToPath(sl); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; - output.path_candidate = shifted_path.path; + const uint16_t steering_factor_direction = std::invoke([&output]() { + return output.lateral_shift > 0.0 ? SteeringFactor::LEFT : SteeringFactor::RIGHT; + }); + steering_factor_interface_ptr_->updateSteeringFactor( + {sl_front.start, sl_back.end}, + {output.start_distance_to_path_change, output.finish_distance_to_path_change}, + SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, + ""); + output.path_candidate = shifted_path.path; return output; } @@ -2286,7 +2144,7 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - current_raw_shift_lines_ = avoid_data_.unapproved_raw_sl; + current_raw_shift_lines_ = avoid_data_.raw_shift_line; registerRawShiftLines(shift_lines); @@ -2343,7 +2201,7 @@ void AvoidanceModule::addNewShiftLines( continue; } - if (sl.end_idx >= new_shift_end_idx) { + if (sl.end_idx > new_shift_end_idx) { if ( sl.end_shift_length > -1e-3 && new_shift_length > -1e-3 && sl.end_shift_length < new_shift_length) { @@ -2375,18 +2233,19 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setLateralAccelerationLimit(helper_.getLateralMaxAccelLimit()); } -AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidates) const +AvoidLineArray AvoidanceModule::findNewShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const { - if (candidates.empty()) { + if (shift_lines.empty()) { return {}; } // add small shift lines. const auto add_straight_shift = [&, this](auto & subsequent, bool has_large_shift, const size_t start_idx) { - for (size_t i = start_idx; i < candidates.size(); ++i) { + for (size_t i = start_idx; i < shift_lines.size(); ++i) { if ( - std::abs(candidates.at(i).getRelativeLength()) > + std::abs(shift_lines.at(i).getRelativeLength()) > parameters_->lateral_small_shift_threshold) { if (has_large_shift) { return; @@ -2395,37 +2254,38 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat has_large_shift = true; } - if (!isComfortable(AvoidLineArray{candidates.at(i)})) { + if (!isComfortable(AvoidLineArray{shift_lines.at(i)})) { return; } - subsequent.push_back(candidates.at(i)); + subsequent.push_back(shift_lines.at(i)); } }; // get subsequent shift lines. const auto get_subsequent_shift = [&, this](size_t i) { - AvoidLineArray subsequent{candidates.at(i)}; + AvoidLineArray subsequent{shift_lines.at(i)}; if (!isComfortable(subsequent)) { return subsequent; } - if (candidates.size() == i + 1) { + if (shift_lines.size() == i + 1) { return subsequent; } - if (!isComfortable(AvoidLineArray{candidates.at(i + 1)})) { + if (!isComfortable(AvoidLineArray{shift_lines.at(i + 1)})) { return subsequent; } if ( - std::abs(candidates.at(i).getRelativeLength()) < parameters_->lateral_small_shift_threshold) { + std::abs(shift_lines.at(i).getRelativeLength()) < + parameters_->lateral_small_shift_threshold) { const auto has_large_shift = - candidates.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; + shift_lines.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; // candidate.at(i) is small length shift line. add large length shift line. - subsequent.push_back(candidates.at(i + 1)); + subsequent.push_back(shift_lines.at(i + 1)); add_straight_shift(subsequent, has_large_shift, i + 2); } else { // candidate.at(i) is large length shift line. add small length shift lines. @@ -2440,18 +2300,19 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; }; - for (size_t i = 0; i < candidates.size(); ++i) { - const auto & candidate = candidates.at(i); + for (size_t i = 0; i < shift_lines.size(); ++i) { + const auto & candidate = shift_lines.at(i); // new shift points must exist in front of Ego // this value should be larger than -eps consider path shifter calculation error. - const double eps = 0.01; - if (candidate.start_longitudinal < -eps) { + if (candidate.start_idx < avoid_data_.ego_closest_path_index) { break; } if (!is_ignore_shift(candidate)) { - return get_subsequent_shift(i); + const auto new_shift_lines = get_subsequent_shift(i); + debug.step4_new_shift_line = new_shift_lines; + return new_shift_lines; } } @@ -2473,6 +2334,8 @@ bool AvoidanceModule::isValidShiftLine( ShiftedPath proposed_shift_path; shifter_for_validate.generate(&proposed_shift_path); + debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; + // check offset between new shift path and ego position. { const auto new_idx = planner_data_->findEgoIndex(proposed_shift_path.path.points); @@ -2487,8 +2350,6 @@ bool AvoidanceModule::isValidShiftLine( } } - debug_data_.proposed_spline_shift = proposed_shift_path.shift_length; - return true; // valid shift line. } @@ -2578,7 +2439,7 @@ void AvoidanceModule::updateRTCData() updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); - const auto candidates = data.safe ? data.safe_new_sl : data.unapproved_new_sl; + const auto candidates = data.safe ? data.safe_shift_line : data.new_shift_line; if (candidates.empty()) { removeCandidateRTCStatus(); @@ -2734,6 +2595,8 @@ void AvoidanceModule::updateDebugMarker( return; } + const auto & path = data.reference_path; + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; @@ -2752,20 +2615,16 @@ void AvoidanceModule::updateDebugMarker( add(createOtherObjectsMarkerArray(objects, ns)); }; - add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - - const auto & path = data.reference_path; - add(createPathMarkerArray(debug.center_line, "centerline", 0, 0.0, 0.5, 0.9)); - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createPathMarkerArray( - helper_.getPreviousLinearShiftPath().path, "prev_linear_shift", 0, 0.5, 0.4, 0.6)); - add(createPoseMarkerArray(data.reference_pose, "reference_pose", 0, 0.9, 0.3, 0.3)); - - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); + const auto addShiftLength = + [&](const auto & shift_length, const auto & ns, auto r, auto g, auto b) { + add(createShiftLengthMarkerArray(shift_length, path, ns, r, g, b)); + }; - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + const auto addShiftGrad = [&]( + const auto & shift_grad, const auto & shift_length, const auto & ns, + auto r, auto g, auto b) { + add(createShiftGradMarkerArray(shift_grad, shift_length, path, ns, r, g, b)); + }; // ignore objects { @@ -2782,11 +2641,32 @@ void AvoidanceModule::updateDebugMarker( addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); } - // parent object info + // shift line pre-process { - addAvoidLine(debug.registered_raw_shift, "p_registered_shift", 0.8, 0.8, 0.0); - addAvoidLine(debug.current_raw_shift, "p_current_raw_shift", 0.5, 0.2, 0.2); - addAvoidLine(debug.extra_return_shift, "p_extra_return_shift", 0.0, 0.5, 0.8); + addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); + addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); + addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); + addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.2, 0.8, 0.4, 0.3); + addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.2, 1.0, 0.2, 0.3); + } + + // merge process + { + addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); + } + + // trimming process + { + addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); + } + + // registering process + { + addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); + addAvoidLine(data.raw_shift_line, "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3); + addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); } // safety check @@ -2798,58 +2678,28 @@ void AvoidanceModule::updateDebugMarker( // shift length { - const std::string ns = "shift_length"; - add(createShiftLengthMarkerArray(debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftLengthMarkerArray(debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftLengthMarkerArray(debug.total_shift, path, ns + "_total", 0.99, 0.4, 0.2)); + addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); + addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); + addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); } // shift grad { - const std::string ns = "shift_grad"; - add(createShiftGradMarkerArray( - debug.pos_shift_grad, debug.pos_shift, path, ns + "_pos", 0.0, 0.7, 0.5)); - add(createShiftGradMarkerArray( - debug.neg_shift_grad, debug.neg_shift, path, ns + "_neg", 0.0, 0.5, 0.7)); - add(createShiftGradMarkerArray( - debug.total_forward_grad, debug.total_shift, path, ns + "_total_forward", 0.99, 0.4, 0.2)); - add(createShiftGradMarkerArray( - debug.total_backward_grad, debug.total_shift, path, ns + "_total_backward", 0.4, 0.2, 0.99)); - } - - // shift path - { - const std::string ns = "shift_line"; - add(createShiftLengthMarkerArray( - helper_.getPreviousLinearShiftPath().shift_length, path, ns + "_linear_registered", 0.9, 0.3, - 0.3)); - add(createShiftLengthMarkerArray( - debug.proposed_spline_shift, path, ns + "_spline_proposed", 1.0, 1.0, 1.0)); + addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); + addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); + addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); + addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); } - // child shift points + // misc { - const std::string ns = "pipeline"; - add(createAvoidLineMarkerArray(debug.gap_filled, ns + "_1_gap_filled", 0.5, 0.8, 1.0, 0.05)); - add(createAvoidLineMarkerArray(debug.merged, ns + "_2_merge", 0.345, 0.968, 1.0, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift, ns + "_3_concat_by_grad", 0.976, 0.328, 0.910, 0.05)); - add( - createAvoidLineMarkerArray(debug.quantized, ns + "_4_quantized", 0.505, 0.745, 0.969, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_small_shift, ns + "_5_trim_small_shift", 0.663, 0.525, 0.941, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_second, ns + "_6_concat_by_grad", 0.97, 0.32, 0.91, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_momentary_return, ns + "_7_trim_momentary_return", 0.976, 0.078, 0.878, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_too_sharp_shift, ns + "_8_trim_sharp_shift", 0.576, 0.0, 0.978, 0.05)); - add(createAvoidLineMarkerArray( - debug.trim_similar_grad_shift_third, ns + "_9_concat_by_grad", 1.0, 0.0, 0.0, 0.05)); - } - - addShiftLine(shifter.getShiftLines(), "path_shifter_registered_points", 0.99, 0.99, 0.0, 0.5); - addAvoidLine(debug.new_shift_lines, "path_shifter_proposed_points", 0.99, 0.0, 0.0, 0.5); + add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); + add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); + add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + } } void AvoidanceModule::updateAvoidanceDebugData( @@ -2903,6 +2753,77 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const return object.longitudinal - std::min(variable + constant, p->stop_max_distance); } +void AvoidanceModule::insertReturnDeadLine( + const bool use_constraints_for_decel, ShiftedPath & shifted_path) const +{ + const auto & data = avoid_data_; + + if (!planner_data_->route_handler->isInGoalRouteSection(data.current_lanelets.back())) { + RCLCPP_DEBUG(getLogger(), "goal is far enough."); + return; + } + + const auto shift_length = path_shifter_.getLastShiftLength(); + + if (std::abs(shift_length) < 1e-3) { + RCLCPP_DEBUG(getLogger(), "don't have to consider return shift."); + return; + } + + const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + + const auto to_goal = calcSignedArcLength( + shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); + const auto to_stop_line = to_goal - min_return_distance - parameters_->remain_buffer_distance; + + // If we don't need to consider deceleration constraints, insert a deceleration point + // and return immediately + if (!use_constraints_for_decel) { + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, + stop_pose_); + return; + } + + // If the stop distance is not enough for comfortable stop, don't insert wait point. + const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < to_stop_line; + if (!is_comfortable_stop) { + RCLCPP_DEBUG(getLogger(), "stop distance is not enough."); + return; + } + + utils::avoidance::insertDecelPoint( + getEgoPosition(), to_stop_line - parameters_->stop_buffer, 0.0, shifted_path.path, stop_pose_); + + // insert slow down speed. + const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), to_stop_line); + if (current_target_velocity < getEgoSpeed()) { + RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); + return; + } + + const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points); + for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) { + const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i); + + // slow down speed is inserted only in front of the object. + const auto shift_longitudinal_distance = to_stop_line - distance_from_ego; + if (shift_longitudinal_distance < 0.0) { + break; + } + + // target speed with nominal jerk limits. + const double v_target = PathShifter::calcFeasibleVelocityFromJerk( + shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; + const double v_insert = + std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); + + shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_insert); + } +} + void AvoidanceModule::insertWaitPoint( const bool use_constraints_for_decel, ShiftedPath & shifted_path) const { @@ -3033,6 +2954,12 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto object = data.target_objects.front(); + const auto enough_space = + object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK; + if (!enough_space) { + return; + } + // calculate shift length for front object. const auto & vehicle_width = planner_data_->parameters.vehicle_width; const auto object_type = utils::getHighestProbLabel(object.object.classification); diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index b5ecffbcc9..181b9fbfd5 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -22,13 +22,17 @@ #include #include +#include + +#include +#include + #include #include #include #include #include #include - namespace behavior_path_planner { namespace diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 12f6f243e9..1f5a823386 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -24,6 +24,7 @@ #include "tier4_autoware_utils/math/unit_conversion.hpp" #include +#include #include #include #include @@ -72,19 +73,19 @@ GoalPlannerModule::GoalPlannerModule( pull_over_planners_.push_back(std::make_shared( node, *parameters, lane_departure_checker, occupancy_grid_map_)); } - // currently only support geometric_parallel_parking for left side parking - if (left_side_parking_) { - if (parameters_->enable_arc_forward_parking) { - constexpr bool is_forward = true; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); - } - if (parameters_->enable_arc_backward_parking) { - constexpr bool is_forward = false; - pull_over_planners_.push_back(std::make_shared( - node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); - } + + // set geometric pull over + if (parameters_->enable_arc_forward_parking) { + constexpr bool is_forward = true; + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); } + if (parameters_->enable_arc_backward_parking) { + constexpr bool is_forward = false; + pull_over_planners_.push_back(std::make_shared( + node, *parameters, lane_departure_checker, occupancy_grid_map_, is_forward)); + } + if (pull_over_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); } @@ -182,13 +183,13 @@ void GoalPlannerModule::onTimer() }; // plan candidate paths and set them to the member variable - if (parameters_->search_priority == "efficient_path") { + if (parameters_->path_priority == "efficient_path") { for (const auto & planner : pull_over_planners_) { for (const auto & goal_candidate : goal_candidates) { planCandidatePaths(planner, goal_candidate); } } - } else if (parameters_->search_priority == "close_goal") { + } else if (parameters_->path_priority == "close_goal") { for (const auto & goal_candidate : goal_candidates) { for (const auto & planner : pull_over_planners_) { planCandidatePaths(planner, goal_candidate); @@ -196,9 +197,9 @@ void GoalPlannerModule::onTimer() } } else { RCLCPP_ERROR( - getLogger(), "search_priority should be efficient_path or close_goal, but %s is given.", - parameters_->search_priority.c_str()); - throw std::domain_error("[pull_over] invalid search_priority"); + getLogger(), "path_priority should be efficient_path or close_goal, but %s is given.", + parameters_->path_priority.c_str()); + throw std::domain_error("[pull_over] invalid path_priority"); } // set member variables @@ -264,7 +265,9 @@ void GoalPlannerModule::initializeSafetyCheckParameters() void GoalPlannerModule::processOnEntry() { // Initialize occupancy grid map - if (parameters_->use_occupancy_grid) { + if ( + parameters_->use_occupancy_grid_for_goal_search || + parameters_->use_occupancy_grid_for_path_collision_check) { initializeOccupancyGridMap(); } // Initialize safety checker @@ -448,13 +451,6 @@ Pose GoalPlannerModule::calcRefinedGoal(const Pose & goal_pose) const ModuleStatus GoalPlannerModule::updateState() { - // finish module only when the goal is fixed - if ( - !goal_planner_utils::isAllowedGoalModification(planner_data_->route_handler) && - hasFinishedGoalPlanner()) { - return ModuleStatus::SUCCESS; - } - // start_planner module will be run when setting new goal, so not need finishing pull_over module. // Finishing it causes wrong lane_following path generation. return current_state_; @@ -663,29 +659,29 @@ void GoalPlannerModule::setLanes() void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) { - if (status_.is_safe_static_objects) { - if (isSafePath()) { - // clear stop pose when the path is safe against static/dynamic objects and activated - if (isActivated()) { - resetWallPoses(); - } - // keep stop if not enough time passed, - // because it takes time for the trajectory to be reflected - auto current_path = getCurrentPath(); - keepStoppedWithCurrentPath(current_path); - - output.path = std::make_shared(current_path); - output.reference_path = getPreviousModuleOutput().reference_path; - } else if (status_.has_decided_path && isActivated()) { - // situation : not safe against dynamic objects after approval - // insert stop point in current path if ego is able to stop with acceleration and jerk - // constraints - setStopPathFromCurrentPath(output); + if (!status_.is_safe_static_objects) { + // situation : not safe against static objects use stop_path + setStopPath(output); + } else if ( + parameters_->safety_check_params.enable_safety_check && !isSafePath() && + status_.has_decided_path && isActivated()) { + // situation : not safe against dynamic objects after approval + // insert stop point in current path if ego is able to stop with acceleration and jerk + // constraints + setStopPathFromCurrentPath(output); + } else { + // situation : (safe against static and dynamic objects) or (safe against static objects and + // before approval) don't stop + if (isActivated()) { + resetWallPoses(); } + // keep stop if not enough time passed, + // because it takes time for the trajectory to be reflected + auto current_path = getCurrentPath(); + keepStoppedWithCurrentPath(current_path); - } else { - // not safe: use stop_path - setStopPath(output); + output.path = std::make_shared(current_path); + output.reference_path = getPreviousModuleOutput().reference_path; } setDrivableAreaInfo(output); @@ -700,7 +696,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. status_.prev_is_safe = status_.is_safe_static_objects; - status_.prev_is_safe_dynamic_objects = status_.is_safe_dynamic_objects; + status_.prev_is_safe_dynamic_objects = + parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; } void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) @@ -732,8 +729,13 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path if (status_.prev_is_safe_dynamic_objects || status_.prev_stop_path_after_approval == nullptr) { - if (const auto stop_inserted_path = generateStopInsertedCurrentPath()) { - output.path = std::make_shared(*stop_inserted_path); + auto current_path = getCurrentPath(); + const auto stop_path = + behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( + current_path, planner_data_, *stop_pose_, parameters_->maximum_deceleration_for_stop, + parameters_->maximum_jerk_for_stop); + if (stop_path) { + output.path = std::make_shared(*stop_path); status_.prev_stop_path_after_approval = output.path; RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "Collision detected, generate stop path"); } else { @@ -1111,34 +1113,6 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath() return stop_path; } -std::optional GoalPlannerModule::generateStopInsertedCurrentPath() -{ - auto current_path = getCurrentPath(); - if (current_path.points.empty()) { - return {}; - } - - // try to insert stop point in current_path after approval - // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point - const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); - if (!min_stop_distance) { - return {}; - } - - // set stop point - const auto stop_idx = motion_utils::insertStopPoint( - planner_data_->self_odometry->pose.pose, *min_stop_distance, current_path.points); - - if (!stop_idx) { - return {}; - } else { - stop_pose_ = current_path.points.at(*stop_idx).point.pose; - } - - return current_path; -} - void GoalPlannerModule::transitionToNextPathIfFinishingCurrentPath() { if (isActivated() && last_approved_time_ != nullptr) { @@ -1246,11 +1220,6 @@ bool GoalPlannerModule::isOnModifiedGoal() const parameters_->th_arrived_distance; } -bool GoalPlannerModule::hasFinishedGoalPlanner() -{ - return isOnModifiedGoal() && isStopped(); -} - TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output @@ -1292,7 +1261,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo() const bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const { - if (parameters_->use_occupancy_grid || !occupancy_grid_map_) { + if (parameters_->use_occupancy_grid_for_path_collision_check && occupancy_grid_map_) { const bool check_out_of_range = false; if (occupancy_grid_map_->hasObstacleOnPath(path, check_out_of_range)) { return true; @@ -1300,12 +1269,20 @@ bool GoalPlannerModule::checkCollision(const PathWithLaneId & path) const } if (parameters_->use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, + parameters_->forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_->th_moving_object_velocity); const auto common_parameters = planner_data_->parameters; const double base_link2front = common_parameters.base_link2front; const double base_link2rear = common_parameters.base_link2rear; const double vehicle_width = common_parameters.vehicle_width; if (utils::path_safety_checker::checkCollisionWithExtraStoppingMargin( - path, *(planner_data_->dynamic_object), base_link2front, base_link2rear, vehicle_width, + path, pull_over_lane_stop_objects, base_link2front, base_link2rear, vehicle_width, parameters_->maximum_deceleration, parameters_->object_recognition_collision_check_margin, parameters_->object_recognition_collision_check_max_extra_stopping_margin)) { return true; @@ -1575,10 +1552,13 @@ bool GoalPlannerModule::isSafePath() const RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); + // TODO(Sugahara): shoule judge is_object_front properly + const bool is_object_front = true; + const bool limit_to_max_velocity = true; const auto ego_predicted_path = behavior_path_planner::utils::path_safety_checker::createPredictedPath( ego_predicted_path_params_, pull_over_path.points, current_pose, current_velocity, - ego_seg_idx); + ego_seg_idx, is_object_front, limit_to_max_velocity); // filtering objects with velocity, position and class const auto & filtered_objects = utils::path_safety_checker::filterObjects( @@ -1590,7 +1570,7 @@ bool GoalPlannerModule::isSafePath() const pull_over_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.prev_is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1634,6 +1614,9 @@ void GoalPlannerModule::setDebugData() using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; using marker_utils::createPredictedPathMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; @@ -1675,17 +1658,26 @@ void GoalPlannerModule::setDebugData() add( createPathMarkerArray(partial_path, "partial_path_" + std::to_string(i), 0, 0.9, 0.5, 0.9)); } + } + // safety check + if (parameters_->safety_check_params.enable_safety_check) { if (goal_planner_data_.ego_predicted_path.size() > 0) { const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); add(createPredictedPathMarkerArray( - ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); + ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); } if (goal_planner_data_.filtered_objects.objects.size() > 0) { add(createObjectsMarkerArray( goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } + + add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); + add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + goal_planner_data_.collision_check); } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index ba147a2817..79459d6efb 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -38,12 +38,17 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.th_stopped_velocity = node->declare_parameter(base_ns + "th_stopped_velocity"); p.th_arrived_distance = node->declare_parameter(base_ns + "th_arrived_distance"); p.th_stopped_time = node->declare_parameter(base_ns + "th_stopped_time"); + p.center_line_path_interval = + node->declare_parameter(base_ns + "center_line_path_interval"); } // goal search { const std::string ns = base_ns + "goal_search."; - p.search_priority = node->declare_parameter(ns + "search_priority"); + p.goal_priority = node->declare_parameter(ns + "goal_priority"); + p.minimum_weighted_distance_lateral_weight = + node->declare_parameter(ns + "minimum_weighted_distance.lateral_weight"); + p.path_priority = node->declare_parameter(ns + "path_priority"); p.forward_goal_search_length = node->declare_parameter(ns + "forward_goal_search_length"); p.backward_goal_search_length = @@ -72,9 +77,12 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // occupancy grid map { const std::string ns = base_ns + "occupancy_grid."; - p.use_occupancy_grid = node->declare_parameter(ns + "use_occupancy_grid"); - p.use_occupancy_grid_for_longitudinal_margin = - node->declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + p.use_occupancy_grid_for_goal_search = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_search"); + p.use_occupancy_grid_for_path_collision_check = + node->declare_parameter(ns + "use_occupancy_grid_for_path_collision_check"); + p.use_occupancy_grid_for_goal_longitudinal_margin = + node->declare_parameter(ns + "use_occupancy_grid_for_goal_longitudinal_margin"); p.occupancy_grid_collision_check_margin = node->declare_parameter(ns + "occupancy_grid_collision_check_margin"); p.theta_size = node->declare_parameter(ns + "theta_size"); @@ -90,6 +98,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.object_recognition_collision_check_max_extra_stopping_margin = node->declare_parameter( ns + "object_recognition_collision_check_max_extra_stopping_margin"); + p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); } // pull over general params @@ -117,6 +126,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(ns + "after_shift_straight_distance"); } + // parallel parking common + { + const std::string ns = base_ns + "pull_over.parallel_parking."; + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking + } + // forward parallel parking forward { const std::string ns = base_ns + "pull_over.parallel_parking.forward."; @@ -232,18 +248,20 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // EgoPredictedPath std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; { + p.ego_predicted_path_params.min_velocity = + node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.time_horizon = - node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.max_velocity = + node->declare_parameter(ego_path_ns + "max_velocity"); + p.ego_predicted_path_params.time_horizon_for_front_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = node->declare_parameter(ego_path_ns + "time_resolution"); - p.ego_predicted_path_params.min_slow_speed = - node->declare_parameter(ego_path_ns + "min_slow_speed"); p.ego_predicted_path_params.delay_until_departure = node->declare_parameter(ego_path_ns + "delay_until_departure"); - p.ego_predicted_path_params.target_velocity = - node->declare_parameter(ego_path_ns + "target_velocity"); } // ObjectFilteringParams @@ -312,6 +330,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index a8ff0c0bbc..6d06b26cd1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -19,6 +19,10 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include +#include + +#include +#include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 74af83c2db..51875952d4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -15,6 +15,7 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/marker_utils/lane_change/debug.hpp" +#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" @@ -293,10 +294,12 @@ void LaneChangeInterface::setObjectDebugVisualization() const using marker_utils::showPredictedPath; using marker_utils::showSafetyCheckInfo; using marker_utils::lane_change_markers::showAllValidLaneChangePath; + using marker_utils::lane_change_markers::showFilteredObjects; const auto debug_data = module_type_->getDebugData(); const auto debug_after_approval = module_type_->getAfterApprovalDebugData(); const auto debug_valid_path = module_type_->getDebugValidPath(); + const auto debug_filtered_objects = module_type_->getDebugFilteredObjects(); debug_marker_.markers.clear(); const auto add = [this](const MarkerArray & added) { @@ -304,6 +307,9 @@ void LaneChangeInterface::setObjectDebugVisualization() const }; add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths")); + add(showFilteredObjects( + debug_filtered_objects.current_lane, debug_filtered_objects.target_lane, + debug_filtered_objects.other_lane, "object_filtered")); if (!debug_data.empty()) { add(showSafetyCheckInfo(debug_data, "object_debug_info")); add(showPredictedPath(debug_data, "ego_predicted_path")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index f4a75daa6c..a502c9d8e4 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -78,6 +78,17 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.use_all_predicted_path = getOrDeclareParameter(*node, parameter("use_all_predicted_path")); + // lane change regulations + p.regulate_on_crosswalk = getOrDeclareParameter(*node, parameter("regulation.crosswalk")); + p.regulate_on_intersection = + getOrDeclareParameter(*node, parameter("regulation.intersection")); + + // ego vehicle stuck detection + p.stop_velocity_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.velocity")); + p.stop_time_threshold = + getOrDeclareParameter(*node, parameter("stuck_detection.stop_time")); + p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter( *node, parameter("safety_check.longitudinal_distance_min_threshold")); p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter( @@ -111,14 +122,16 @@ LaneChangeModuleManager::LaneChangeModuleManager( // target object { std::string ns = "lane_change.target_object."; - p.check_car = getOrDeclareParameter(*node, ns + "car"); - p.check_truck = getOrDeclareParameter(*node, ns + "truck"); - p.check_bus = getOrDeclareParameter(*node, ns + "bus"); - p.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); - p.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); - p.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); - p.check_motorcycle = getOrDeclareParameter(*node, ns + "motorcycle"); - p.check_pedestrian = getOrDeclareParameter(*node, ns + "pedestrian"); + p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); + p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); + p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); + p.object_types_to_check.check_trailer = getOrDeclareParameter(*node, ns + "trailer"); + p.object_types_to_check.check_unknown = getOrDeclareParameter(*node, ns + "unknown"); + p.object_types_to_check.check_bicycle = getOrDeclareParameter(*node, ns + "bicycle"); + p.object_types_to_check.check_motorcycle = + getOrDeclareParameter(*node, ns + "motorcycle"); + p.object_types_to_check.check_pedestrian = + getOrDeclareParameter(*node, ns + "pedestrian"); } // lane change cancel diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index a81f6227e4..d1de12a3bd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -25,6 +25,9 @@ #include #include +#include +#include + #include #include #include @@ -40,10 +43,12 @@ NormalLaneChange::NormalLaneChange( : LaneChangeBase(parameters, type, direction) { stop_watch_.tic(getModuleTypeStr()); + stop_watch_.tic("stop_time"); } void NormalLaneChange::updateLaneChangeStatus() { + updateStopTime(); const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -564,19 +569,26 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( return utils::lane_change::getAccelerationValues(min_acc, max_acc, longitudinal_acc_sampling_num); } -double NormalLaneChange::calcPrepareDuration( +std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { const auto & common_parameters = planner_data_->parameters; - const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation; - const auto current_vel = getEgoVelocity(); + const auto base_link2front = planner_data_->parameters.base_link2front; + const auto threshold = + lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; + + std::vector prepare_durations; + constexpr double step = 0.5; - // if the ego vehicle is close to the end of the lane at a low speed - if (isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold) && current_vel < 1.0) { - return 0.0; + for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + duration -= step) { + prepare_durations.push_back(duration); + if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { + break; + } } - return common_parameters.lane_change_prepare_duration; + return prepare_durations; } PathWithLaneId NormalLaneChange::getPrepareSegment( @@ -664,23 +676,30 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); const auto target_polygon = utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); - const auto target_backward_polygon = utils::lane_change::createPolygon( - target_backward_lanes, 0.0, std::numeric_limits::max()); + const auto dist_ego_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); + std::vector> target_backward_polygons; + for (const auto & target_backward_lane : target_backward_lanes) { + lanelet::ConstLanelets lanelet{target_backward_lane}; + auto lane_polygon = + utils::lane_change::createPolygon(lanelet, 0.0, std::numeric_limits::max()); + target_backward_polygons.push_back(lane_polygon); + } + + auto filtered_objects = objects; + + utils::path_safety_checker::filterObjectsByClass( + filtered_objects, lane_change_parameters_->object_types_to_check); LaneChangeTargetObjectIndices filtered_obj_indices; - for (size_t i = 0; i < objects.objects.size(); ++i) { - const auto & object = objects.objects.at(i); + for (size_t i = 0; i < filtered_objects.objects.size(); ++i) { + const auto & object = filtered_objects.objects.at(i); const auto & obj_velocity_norm = std::hypot( object.kinematics.initial_twist_with_covariance.twist.linear.x, object.kinematics.initial_twist_with_covariance.twist.linear.y); const auto extended_object = utils::lane_change::transform(object, common_parameters, *lane_change_parameters_); - // ignore specific object types - if (!utils::lane_change::isTargetObjectType(object, *lane_change_parameters_)) { - continue; - } - const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object); // calc distance from the current ego position @@ -694,6 +713,14 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( min_dist_ego_to_obj = std::min(dist_ego_to_obj, min_dist_ego_to_obj); } + const auto is_lateral_far = [&]() { + const auto dist_object_to_current_lanes_center = + lanelet::utils::getLateralDistanceToClosestLanelet( + current_lanes, object.kinematics.initial_pose_with_covariance.pose); + const auto lateral = dist_object_to_current_lanes_center - dist_ego_to_current_lanes_center; + return std::abs(lateral) > (common_parameters.vehicle_width / 2); + }; + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -704,14 +731,22 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( // TODO(watanabe): ignore static parked object that are in front of the ego vehicle in target // lanes - filtered_obj_indices.target_lane.push_back(i); - continue; + if (max_dist_ego_to_obj >= 0 || is_lateral_far()) { + filtered_obj_indices.target_lane.push_back(i); + continue; + } } + const auto check_backward_polygon = [&obj_polygon](const auto & target_backward_polygon) { + return target_backward_polygon && + boost::geometry::intersects(target_backward_polygon.value(), obj_polygon); + }; + // check if the object intersects with target backward lanes if ( - target_backward_polygon && - boost::geometry::intersects(target_backward_polygon.value(), obj_polygon)) { + !target_backward_polygons.empty() && + std::any_of( + target_backward_polygons.begin(), target_backward_polygons.end(), check_backward_polygon)) { filtered_obj_indices.target_lane.push_back(i); continue; } @@ -785,6 +820,7 @@ bool NormalLaneChange::hasEnoughLength( { const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = @@ -818,6 +854,45 @@ bool NormalLaneChange::hasEnoughLength( return true; } +bool NormalLaneChange::hasEnoughLengthToCrosswalk( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_crosswalk_from_lane_change_start_pose = + utils::getDistanceToCrosswalk(current_pose, current_lanes, *overall_graphs_ptr) - + path.info.length.prepare; + // Check lane changing section includes crosswalk + if ( + dist_to_crosswalk_from_lane_change_start_pose > 0.0 && + dist_to_crosswalk_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + + return true; +} + +bool NormalLaneChange::hasEnoughLengthToIntersection( + const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const +{ + const auto current_pose = getEgoPose(); + const auto & route_handler = *getRouteHandler(); + const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); + + const double dist_to_intersection_from_lane_change_start_pose = + utils::getDistanceToNextIntersection(current_pose, current_lanes) - path.info.length.prepare; + // Check lane changing section includes intersection + if ( + dist_to_intersection_from_lane_change_start_pose > 0.0 && + dist_to_intersection_from_lane_change_start_pose < path.info.length.lane_changing) { + return false; + } + + return true; +} + bool NormalLaneChange::getLaneChangePaths( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const @@ -860,183 +935,209 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; - candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); + const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); - const auto prepare_duration = calcPrepareDuration(current_lanes, target_lanes); + candidate_paths->reserve( + longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num * prepare_durations.size()); - for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { - // get path on original lanes - const auto prepare_velocity = std::max( - current_velocity + sampled_longitudinal_acc * prepare_duration, - minimum_lane_changing_velocity); + for (const auto & prepare_duration : prepare_durations) { + for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { + // get path on original lanes + const auto prepare_velocity = std::max( + current_velocity + sampled_longitudinal_acc * prepare_duration, + minimum_lane_changing_velocity); - // compute actual longitudinal acceleration - const double longitudinal_acc_on_prepare = - (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); + // compute actual longitudinal acceleration + const double longitudinal_acc_on_prepare = + (prepare_duration < 1e-3) ? 0.0 + : ((prepare_velocity - current_velocity) / prepare_duration); - const double prepare_length = current_velocity * prepare_duration + - 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); + const double prepare_length = + current_velocity * prepare_duration + + 0.5 * longitudinal_acc_on_prepare * std::pow(prepare_duration, 2); - auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); + auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); - if (prepare_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); - continue; - } - - // lane changing start getEgoPose() is at the end of prepare segment - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( - current_lanes, target_lanes.front(), lane_changing_start_pose); + if (prepare_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "prepare segment is empty!!"); + continue; + } - // Check if the lane changing start point is not on the lanes next to target lanes, - if (target_length_from_lane_change_start_pose > 0.0) { - RCLCPP_DEBUG( - logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); - break; - } + // lane changing start getEgoPose() is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose); - const auto shift_length = - lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); - - const auto initial_lane_changing_velocity = prepare_velocity; - const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; - - // get lateral acceleration range - const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); - const auto lateral_acc_resolution = - std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; - constexpr double lateral_acc_epsilon = 0.01; - - for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; - lateral_acc += lateral_acc_resolution) { - const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); - const double longitudinal_acc_on_lane_changing = - utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, - sampled_longitudinal_acc); - const auto lane_changing_length = - initial_lane_changing_velocity * lane_changing_time + - 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; - const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; - utils::lane_change::setPrepareVelocity( - prepare_segment, current_velocity, terminal_lane_changing_velocity); - - if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { - RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); - continue; + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG( + logger_, "[only new arch] lane change start getEgoPose() is behind target lanelet!!"); + break; } - if (is_goal_in_route) { - const double s_start = - lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; - const double s_goal = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; - const auto num = - std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); - const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; - if ( - s_start + lane_changing_length + finish_judge_buffer + backward_buffer + - next_lane_change_buffer > - s_goal) { + const auto shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanes, lane_changing_start_pose); + + const auto initial_lane_changing_velocity = prepare_velocity; + const auto max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; + + // get lateral acceleration range + const auto [min_lateral_acc, max_lateral_acc] = + common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + const auto lateral_acc_resolution = + std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; + constexpr double lateral_acc_epsilon = 0.01; + + for (double lateral_acc = min_lateral_acc; + lateral_acc < max_lateral_acc + lateral_acc_epsilon; + lateral_acc += lateral_acc_resolution) { + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + const double longitudinal_acc_on_lane_changing = + utils::lane_change::calcLaneChangingAcceleration( + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, + sampled_longitudinal_acc); + const auto lane_changing_length = + initial_lane_changing_velocity * lane_changing_time + + 0.5 * longitudinal_acc_on_lane_changing * lane_changing_time * lane_changing_time; + const auto terminal_lane_changing_velocity = + initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time; + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); + + if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); continue; } - } - const auto target_segment = getTargetSegment( - target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, - initial_lane_changing_velocity, next_lane_change_buffer); + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + const auto num = + std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); + const double backward_buffer = + num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + if ( + s_start + lane_changing_length + finish_judge_buffer + backward_buffer + + next_lane_change_buffer > + s_goal) { + RCLCPP_DEBUG(logger_, "length of lane changing path is longer than length to goal!!"); + continue; + } + } - if (target_segment.points.empty()) { - RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); - continue; - } + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose, target_lane_length, lane_changing_length, + initial_lane_changing_velocity, next_lane_change_buffer); - const lanelet::BasicPoint2d lc_start_point( - prepare_segment.points.back().point.pose.position.x, - prepare_segment.points.back().point.pose.position.y); + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "target segment is empty!! something wrong..."); + continue; + } - const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( - target_lanes, 0, std::numeric_limits::max()); - const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + const lanelet::BasicPoint2d lc_start_point( + prepare_segment.points.back().point.pose.position.x, + prepare_segment.points.back().point.pose.position.y); - const auto is_valid_start_point = - boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || - boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + const auto target_lane_polygon = lanelet::utils::getPolygonFromArcLength( + target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); - if (!is_valid_start_point) { - // lane changing points are not inside of the target preferred lanes or its neighbors - continue; - } + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); - const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, initial_lane_changing_velocity); - const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( - route_handler, target_lanes, lane_changing_start_pose, target_lane_length, - lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, - next_lane_change_buffer); + if (!is_valid_start_point) { + // lane changing points are not inside of the target preferred lanes or its neighbors + continue; + } - if (target_lane_reference_path.points.empty()) { - RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); - continue; - } + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_changing_length, initial_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose, target_lane_length, + lane_changing_length, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; - lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( - prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; - - const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); - - if (!candidate_path) { - RCLCPP_DEBUG(logger_, "no candidate path!!"); - continue; - } + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "target_lane_reference_path is empty!!"); + continue; + } - const auto is_valid = - hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction); + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; + lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + prepare_segment, target_segment, target_lane_reference_path, shift_length); + lane_change_info.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + + const auto candidate_path = utils::lane_change::constructCandidatePath( + lane_change_info, prepare_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + if (!candidate_path) { + RCLCPP_DEBUG(logger_, "no candidate path!!"); + continue; + } - if (!is_valid) { - RCLCPP_DEBUG(logger_, "invalid candidate path!!"); - continue; - } + if (!hasEnoughLength(*candidate_path, current_lanes, target_lanes, direction)) { + RCLCPP_DEBUG(logger_, "invalid candidate path!!"); + continue; + } - if (utils::lane_change::passParkedObject( - route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, - is_goal_in_route, *lane_change_parameters_)) { - return false; - } - candidate_paths->push_back(*candidate_path); + if ( + lane_change_parameters_->regulate_on_crosswalk && + !hasEnoughLengthToCrosswalk(*candidate_path, current_lanes)) { + RCLCPP_DEBUG(logger_, "Including crosswalk!!"); + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + continue; + } + RCLCPP_WARN_STREAM( + logger_, "Stop time is over threshold. Allow lane change in crosswalk."); + } - if (!check_safety) { - return false; - } + if ( + lane_change_parameters_->regulate_on_intersection && + !hasEnoughLengthToIntersection(*candidate_path, current_lanes)) { + RCLCPP_DEBUG(logger_, "Including intersection!!"); + if (getStopTime() < lane_change_parameters_->stop_time_threshold) { + continue; + } + RCLCPP_WARN_STREAM( + logger_, "Stop time is over threshold. Allow lane change in intersection."); + } + + if (utils::lane_change::passParkedObject( + route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer, + is_goal_in_route, *lane_change_parameters_)) { + return false; + } + candidate_paths->push_back(*candidate_path); + + if (!check_safety) { + return false; + } - const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( - *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); + const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe( + *candidate_path, target_objects, lane_change_parameters_->rss_params, object_debug_); - if (is_safe) { - return true; + if (is_safe) { + return true; + } } } } @@ -1051,6 +1152,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto & target_lanes = status_.target_lanes; const auto target_objects = getTargetObjects(current_lanes, target_lanes); + debug_filtered_objects_ = target_objects; CollisionCheckDebugMap debug_data; const auto safety_status = isLaneChangePathSafe( @@ -1331,7 +1433,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( lane_change_path, current_twist, current_pose, common_parameters, time_resolution); const auto debug_predicted_path = - utils::lane_change::convertToPredictedPath(ego_predicted_path, time_resolution); + utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); auto collision_check_objects = target_objects.target_lane; @@ -1377,4 +1479,24 @@ void NormalLaneChange::setStopPose(const Pose & stop_pose) lane_change_stop_pose_ = stop_pose; } +void NormalLaneChange::updateStopTime() +{ + const auto current_vel = getEgoVelocity(); + + if (std::abs(current_vel) > lane_change_parameters_->stop_velocity_threshold) { + stop_time_ = 0.0; + } else { + const double duration = stop_watch_.toc("stop_time"); + // clip stop time + if (stop_time_ + duration * 0.001 > lane_change_parameters_->stop_time_threshold) { + constexpr double eps = 0.1; + stop_time_ = lane_change_parameters_->stop_time_threshold + eps; + } else { + stop_time_ += duration * 0.001; + } + } + + stop_watch_.tic("stop_time"); +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index 09166cd72a..30810bda25 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -38,6 +38,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); p.th_turn_signal_on_lateral_offset = node->declare_parameter(ns + "th_turn_signal_on_lateral_offset"); + p.th_distance_to_middle_of_the_road = + node->declare_parameter(ns + "th_distance_to_middle_of_the_road"); p.intersection_search_length = node->declare_parameter(ns + "intersection_search_length"); p.length_ratio_for_turn_signal_deactivation_near_intersection = node->declare_parameter( ns + "length_ratio_for_turn_signal_deactivation_near_intersection"); @@ -45,6 +47,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( p.collision_check_distance_from_end = node->declare_parameter(ns + "collision_check_distance_from_end"); p.th_moving_object_velocity = node->declare_parameter(ns + "th_moving_object_velocity"); + p.center_line_path_interval = node->declare_parameter(ns + "center_line_path_interval"); // shift pull out p.enable_shift_pull_out = node->declare_parameter(ns + "enable_shift_pull_out"); p.check_shift_path_lane_departure = @@ -69,6 +72,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "lane_departure_margin"); p.parallel_parking_parameters.pull_out_max_steer_angle = node->declare_parameter(ns + "pull_out_max_steer_angle"); // 15deg + p.parallel_parking_parameters.center_line_path_interval = + p.center_line_path_interval; // for geometric parallel parking // search start pose backward p.search_priority = node->declare_parameter( ns + "search_priority"); // "efficient_path" or "short_back_distance" @@ -163,18 +168,20 @@ StartPlannerModuleManager::StartPlannerModuleManager( // EgoPredictedPath std::string ego_path_ns = base_ns + "ego_predicted_path."; { + p.ego_predicted_path_params.min_velocity = + node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.time_horizon = - node->declare_parameter(ego_path_ns + "time_horizon"); + p.ego_predicted_path_params.max_velocity = + node->declare_parameter(ego_path_ns + "max_velocity"); + p.ego_predicted_path_params.time_horizon_for_front_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); + p.ego_predicted_path_params.time_horizon_for_rear_object = + node->declare_parameter(ego_path_ns + "time_horizon_for_rear_object"); p.ego_predicted_path_params.time_resolution = node->declare_parameter(ego_path_ns + "time_resolution"); - p.ego_predicted_path_params.min_slow_speed = - node->declare_parameter(ego_path_ns + "min_slow_speed"); p.ego_predicted_path_params.delay_until_departure = node->declare_parameter(ego_path_ns + "delay_until_departure"); - p.ego_predicted_path_params.target_velocity = - node->declare_parameter(ego_path_ns + "target_velocity"); } // ObjectFilteringParams @@ -243,6 +250,8 @@ StartPlannerModuleManager::StartPlannerModuleManager( { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.hysteresis_factor_expand_rate = + node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = node->declare_parameter(safety_check_ns + "backward_path_length"); p.safety_check_params.forward_path_length = diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index a285224404..c2df11b4be 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -21,10 +21,15 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" #include "motion_utils/trajectory/trajectory.hpp" +#include #include #include #include +#include + +#include + #include #include #include @@ -117,10 +122,16 @@ void StartPlannerModule::processOnExit() bool StartPlannerModule::isExecutionRequested() const { - // TODO(Sugahara): if required lateral shift distance is small, don't engage this module. - // Execute when current pose is near route start pose - const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const lanelet::ConstLanelets current_lanes = utils::getCurrentLanes(planner_data_); + const double lateral_distance_to_center_lane = + lanelet::utils::getArcCoordinates(current_lanes, current_pose).distance; + + if (std::abs(lateral_distance_to_center_lane) < parameters_->th_distance_to_middle_of_the_road) { + return false; + } + + const Pose start_pose = planner_data_->route_handler->getOriginalStartPose(); if ( tier4_autoware_utils::calcDistance2d(start_pose.position, current_pose.position) > parameters_->th_arrived_distance) { @@ -181,7 +192,10 @@ ModuleStatus StartPlannerModule::updateState() return ModuleStatus::SUCCESS; } - checkBackFinished(); + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + return ModuleStatus::SUCCESS; // for breaking loop + } return current_state_; } @@ -647,14 +661,28 @@ void StartPlannerModule::updatePullOutStatus() planWithPriority( start_pose_candidates, *refined_start_pose, goal_pose, parameters_->search_priority); - checkBackFinished(); - if (!status_.back_finished) { + if (isBackwardDrivingComplete()) { + updateStatusAfterBackwardDriving(); + current_state_ = ModuleStatus::SUCCESS; // for breaking loop + } else { status_.backward_path = start_planner_utils::getBackwardPath( *route_handler, status_.pull_out_lanes, current_pose, status_.pull_out_start_pose, parameters_->backward_velocity); } } +void StartPlannerModule::updateStatusAfterBackwardDriving() +{ + status_.back_finished = true; + // request start_planner approval + waitApproval(); + // To enable approval of the forward path, the RTC status is removed. + removeRTCStatus(); + for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { + itr->second = generateUUID(); + } +} + PathWithLaneId StartPlannerModule::calcStartPoseCandidatesBackwardPath() const { const Pose & current_pose = planner_data_->self_odometry->pose.pose; @@ -776,9 +804,9 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -void StartPlannerModule::checkBackFinished() +bool StartPlannerModule::isBackwardDrivingComplete() const { - // check ego car is close enough to pull out start pose + // check ego car is close enough to pull out start pose and stopped const auto current_pose = planner_data_->self_odometry->pose.pose; const auto distance = tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); @@ -787,18 +815,12 @@ void StartPlannerModule::checkBackFinished() const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - if (!status_.back_finished && is_near && is_stopped) { + const bool back_finished = !status_.back_finished && is_near && is_stopped; + if (back_finished) { RCLCPP_INFO(getLogger(), "back finished"); - status_.back_finished = true; - - // request start_planner approval - waitApproval(); - removeRTCStatus(); - for (auto itr = uuid_map_.begin(); itr != uuid_map_.end(); ++itr) { - itr->second = generateUUID(); - } - current_state_ = ModuleStatus::SUCCESS; // for breaking loop } + + return back_finished; } bool StartPlannerModule::isStopped() @@ -985,10 +1007,13 @@ bool StartPlannerModule::isSafePath() const RCLCPP_DEBUG(getLogger(), "current_path_idx %ld", status_.current_path_idx); utils::start_goal_planner_common::updatePathProperty( ego_predicted_path_params_, terminal_velocity_and_accel); + // TODO(Sugahara): shoule judge is_object_front properly + const bool is_object_front = true; + const bool limit_to_max_velocity = true; const auto ego_predicted_path = behavior_path_planner::utils::path_safety_checker::createPredictedPath( - ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, - ego_seg_idx); + ego_predicted_path_params_, pull_out_path.points, current_pose, current_velocity, ego_seg_idx, + is_object_front, limit_to_max_velocity); // filtering objects with velocity, position and class const auto & filtered_objects = utils::path_safety_checker::filterObjects( @@ -999,7 +1024,7 @@ bool StartPlannerModule::isSafePath() const current_lanes, route_handler, filtered_objects, objects_filtering_params_); const double hysteresis_factor = - status_.is_safe_dynamic_objects ? 1.0 : parameters_->hysteresis_factor_expand_rate; + status_.is_safe_dynamic_objects ? 1.0 : safety_check_params_->hysteresis_factor_expand_rate; utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); @@ -1146,8 +1171,11 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons DrivableAreaInfo current_drivable_area_info; current_drivable_area_info.drivable_lanes = target_drivable_lanes; - output.drivable_area_info = utils::combineDrivableAreaInfo( - current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); + output.drivable_area_info = + status_.back_finished + ? utils::combineDrivableAreaInfo( + current_drivable_area_info, getPreviousModuleOutput().drivable_area_info) + : current_drivable_area_info; } } @@ -1177,35 +1205,27 @@ void StartPlannerModule::setDebugData() const add(createPoseMarkerArray(status_.pull_out_path.start_pose, "start_pose", 0, 0.3, 0.9, 0.3)); add(createPoseMarkerArray(status_.pull_out_path.end_pose, "end_pose", 0, 0.9, 0.9, 0.3)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); - if (start_planner_data_.ego_predicted_path.size() > 0) { - const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); - add(createPredictedPathMarkerArray( - ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); - } - - if (start_planner_data_.filtered_objects.objects.size() > 0) { - add(createObjectsMarkerArray( - start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); - } + add(createPathMarkerArray(status_.backward_path, "backward_driving_path", 0, 0.0, 0.9, 0.0)); // safety check - { + if (parameters_->safety_check_params.enable_safety_check) { + if (start_planner_data_.ego_predicted_path.size() > 0) { + const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( + start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); + add(createPredictedPathMarkerArray( + ego_predicted_path, vehicle_info_, "ego_predicted_path_start_planner", 0, 0.0, 0.5, 0.9)); + } + + if (start_planner_data_.filtered_objects.objects.size() > 0) { + add(createObjectsMarkerArray( + start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + } + add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); - } - - if (start_planner_data_.ego_predicted_path.size() > 0) { - const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - start_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); - add(createPredictedPathMarkerArray( - ego_predicted_path, vehicle_info_, "ego_predicted_path", 0, 0.0, 0.5, 0.9)); - } - - if (start_planner_data_.filtered_objects.objects.size() > 0) { - add(createObjectsMarkerArray( - start_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); + utils::start_goal_planner_common::initializeCollisionCheckDebugMap( + start_planner_data_.collision_check); } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index 65e3e8343f..3fe30b11e3 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -27,6 +27,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 4afe5381da..b2b4ecd2ea 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -16,6 +16,7 @@ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -28,6 +29,13 @@ #include +#include +#include +#include +#include + +#include + #include #include #include @@ -199,6 +207,22 @@ Polygon2d createOneStepPolygon( return hull_polygon; } + +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 bool isOnRight(const ObjectData & obj) @@ -363,20 +387,7 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine continue; } - // Id the shift is overlapped, insert the shift point. Additionally, the shift which refers - // to the same object id (created by the same object) will be set. - // - // Why? : think that there are two shifts, avoiding and . - // If you register only the avoiding shift, the return-to-center shift will not be generated - // when you get too close to or over the obstacle. The return-shift can be handled with - // addReturnShift(), but it maybe reasonable to register the return-to-center shift for the - // object at the same time as registering the avoidance shift to remove the complexity of the - // addReturnShift(). - for (const auto & al_local : lines1) { - if (al_local.object.object.object_id == al.object.object.object_id) { - ids.insert(al_local.id); - } - } + ids.insert(al.id); } return std::vector(ids.begin(), ids.end()); } @@ -1389,6 +1400,30 @@ void fillAdditionalInfoFromPoint(const AvoidancePlanningData & data, AvoidLineAr } } +void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLine & line) +{ + const auto & path = data.reference_path; + const auto & arc = data.arclength_from_ego; + + line.start_idx = findPathIndexFromArclength(arc, line.start_longitudinal); + line.start = path.points.at(line.start_idx).point.pose; + line.end_idx = findPathIndexFromArclength(arc, line.end_longitudinal); + line.end = path.points.at(line.end_idx).point.pose; +} + +void fillAdditionalInfoFromLongitudinal( + const AvoidancePlanningData & data, AvoidOutlines & outlines) +{ + for (auto & outline : outlines) { + fillAdditionalInfoFromLongitudinal(data, outline.avoid_line); + fillAdditionalInfoFromLongitudinal(data, outline.return_line); + + std::for_each(outline.middle_lines.begin(), outline.middle_lines.end(), [&](auto & line) { + fillAdditionalInfoFromLongitudinal(data, line); + }); + } +} + void fillAdditionalInfoFromLongitudinal(const AvoidancePlanningData & data, AvoidLineArray & lines) { const auto & path = data.reference_path; @@ -1443,34 +1478,29 @@ AvoidLineArray combineRawShiftLinesWithUniqueCheck( std::vector convertToPredictedPath( const PathWithLaneId & path, const std::shared_ptr & planner_data, - const bool is_object_front, const std::shared_ptr & parameters) + const bool is_object_front, const bool limit_to_max_velocity, + const std::shared_ptr & parameters) { - if (path.points.empty()) { - return {}; - } - - const auto & acceleration = parameters->max_acceleration; const auto & vehicle_pose = planner_data->self_odometry->pose.pose; const auto & initial_velocity = std::abs(planner_data->self_odometry->twist.twist.linear.x); - const auto & time_horizon = is_object_front ? parameters->time_horizon_for_front_object - : parameters->time_horizon_for_rear_object; - const auto & time_resolution = parameters->safety_check_time_resolution; - const size_t ego_seg_idx = planner_data->findEgoSegmentIndex(path.points); - std::vector predicted_path; - const auto vehicle_pose_frenet = - convertToFrenetPoint(path.points, vehicle_pose.position, ego_seg_idx); - - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const double velocity = - std::max(initial_velocity + acceleration * t, parameters->min_slow_down_speed); - const double length = initial_velocity * t + 0.5 * acceleration * t * t; - const auto pose = - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length); - predicted_path.emplace_back(t, pose, velocity); - } - return predicted_path; + auto ego_predicted_path_params = + std::make_shared(); + + ego_predicted_path_params->min_velocity = parameters->min_slow_down_speed; + ego_predicted_path_params->acceleration = parameters->max_acceleration; + ego_predicted_path_params->max_velocity = std::numeric_limits::infinity(); + ego_predicted_path_params->time_horizon_for_front_object = + parameters->time_horizon_for_front_object; + ego_predicted_path_params->time_horizon_for_rear_object = + parameters->time_horizon_for_rear_object; + ego_predicted_path_params->time_resolution = parameters->safety_check_time_resolution; + ego_predicted_path_params->delay_until_departure = 0.0; + + return behavior_path_planner::utils::path_safety_checker::createPredictedPath( + ego_predicted_path_params, path.points, vehicle_pose, initial_velocity, ego_seg_idx, + is_object_front, limit_to_max_velocity); } ExtendedPredictedObject transform( @@ -1675,4 +1705,143 @@ std::pair separateObjectsByPath( return std::make_pair(target_objects, other_objects); } + +DrivableLanes generateExpandDrivableLanes( + const lanelet::ConstLanelet & lanelet, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & route_handler = planner_data->route_handler; + + DrivableLanes current_drivable_lanes; + current_drivable_lanes.left_lane = lanelet; + current_drivable_lanes.right_lane = lanelet; + + if (!parameters->use_adjacent_lane) { + return current_drivable_lanes; + } + + // 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, parameters->use_opposite_lane, 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)); + } + }; + const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) { + const auto all_right_lanelets = route_handler->getAllRightSharedLinestringLanelets( + target_lane, parameters->use_opposite_lane, 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)); + } + }; + + update_left_lanelets(lanelet); + update_right_lanelets(lanelet); + + // 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) { + // get previous lane, and return false if previous lane does not exist + lanelet::ConstLanelets prev_lanes; + if (!route_handler->getPreviousLaneletsWithinRoute(lane, &prev_lanes)) { + return lanelet::ConstLanelets{}; + } + + lanelet::ConstLanelets next_lanes; + for (const auto & prev_lane : prev_lanes) { + const auto next_lanes_from_prev = route_handler->getNextLanelets(prev_lane); + pushUniqueVector(next_lanes, next_lanes_from_prev); + } + return next_lanes; + }; + + 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 neighbor 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; + } + + const auto & middle_lanes = current_drivable_lanes.middle_lanes; + const auto has_same_lane = std::any_of( + middle_lanes.begin(), middle_lanes.end(), + [&edge_lane](const auto & lane) { return lane.id() == edge_lane.id(); }); + + if (!has_same_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; + } + 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() != lanelet.id() && + current_drivable_lanes.right_lane.id() != lanelet.id()) { + current_drivable_lanes.middle_lanes.push_back(lanelet); + } + + return current_drivable_lanes; +} } // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index 52be0ac5a8..d0673d8bb9 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index 5b054de43e..3d315e7213 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -22,7 +22,6 @@ #include "tier4_autoware_utils/math/unit_conversion.hpp" #include -#include #include #ifdef ROS_DISTRO_GALACTIC @@ -32,8 +31,12 @@ #include #include + +#include #endif +#include + #include #include #include @@ -118,9 +121,10 @@ void GeometricParallelParking::setVelocityToArcPaths( } std::vector GeometricParallelParking::generatePullOverPaths( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double velocity) + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double velocity) { const double lane_departure_margin = is_forward ? parameters_.forward_parking_lane_departure_margin @@ -128,8 +132,8 @@ std::vector GeometricParallelParking::generatePullOverPaths( const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( - start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - lane_departure_margin, arc_path_interval); + start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, lane_departure_margin, arc_path_interval); if (arc_paths.empty()) { return std::vector{}; } @@ -169,7 +173,8 @@ void GeometricParallelParking::clearPaths() bool GeometricParallelParking::planPullOver( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward) + const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, + const bool left_side_parking) { const auto & common_params = planner_data_->parameters; const double end_pose_offset = is_forward ? -parameters_.after_forward_parking_straight_distance @@ -190,16 +195,16 @@ bool GeometricParallelParking::planPullOver( constexpr double steer_interval = 0.1; for (double steer = parameters_.forward_parking_max_steer_angle; steer > min_steer_rad; steer -= steer_interval) { - const double R_E_r = common_params.wheel_base / std::tan(steer); - const auto start_pose = - calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_r, is_forward); + const double R_E_far = common_params.wheel_base / std::tan(steer); + const auto start_pose = calcStartPose( + arc_end_pose, road_lanes, start_pose_offset, R_E_far, is_forward, left_side_parking); if (!start_pose) { continue; } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_r, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - parameters_.forward_parking_velocity); + *start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, parameters_.forward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -213,15 +218,15 @@ bool GeometricParallelParking::planPullOver( constexpr double offset_interval = 1.0; for (double start_pose_offset = 0; start_pose_offset < max_offset; start_pose_offset += offset_interval) { - const auto start_pose = - calcStartPose(arc_end_pose, road_lanes, start_pose_offset, R_E_min_, is_forward); + const auto start_pose = calcStartPose( + arc_end_pose, road_lanes, start_pose_offset, R_E_min_, is_forward, left_side_parking); if (!start_pose) { continue; } const auto paths = generatePullOverPaths( - *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, end_pose_offset, - parameters_.backward_parking_velocity); + *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_parking, + end_pose_offset, parameters_.backward_parking_velocity); if (!paths.empty()) { paths_ = paths; return true; @@ -234,7 +239,7 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes) + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -245,15 +250,16 @@ bool GeometricParallelParking::planPullOut( end_pose_offset += offset_interval) { // pull_out end pose which is the second arc path end const auto end_pose = - calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward); + calcStartPose(start_pose, road_lanes, end_pose_offset, R_E_min_, is_forward, left_side_start); if (!end_pose) { continue; } // plan reverse path of parking. end_pose <-> start_pose auto arc_paths = planOneTrial( - *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, start_pose_offset, - parameters_.pull_out_lane_departure_margin, parameters_.pull_out_path_interval); + *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, + start_pose_offset, parameters_.pull_out_lane_departure_margin, + parameters_.pull_out_path_interval); if (arc_paths.empty()) { // not found path continue; @@ -280,8 +286,13 @@ bool GeometricParallelParking::planPullOut( s_start, planner_data_->parameters.forward_path_length, road_lanes, goal_pose); const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - PathWithLaneId road_center_line_path = - planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true); + const PathWithLaneId road_center_line_path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath(road_lanes, s_start, s_end, true), + parameters_.center_line_path_interval); + + if (road_center_line_path.points.empty()) { + continue; + } // check the continuity of straight path and arc path const Pose & road_path_first_pose = road_center_line_path.points.front().point.pose; @@ -320,7 +331,7 @@ bool GeometricParallelParking::planPullOut( boost::optional GeometricParallelParking::calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const double start_pose_offset, - const double R_E_r, const bool is_forward) + const double R_E_far, const bool is_forward, const bool left_side_parking) { const auto arc_coordinates = lanelet::utils::getArcCoordinates(road_lanes, goal_pose); @@ -329,7 +340,8 @@ boost::optional GeometricParallelParking::calcStartPose( // But the left turn should also have a minimum turning radius. // see https://www.sciencedirect.com/science/article/pii/S1474667016436852 for the dx detail const double squared_distance_to_arc_connect = - std::pow(R_E_r, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_r, 2); + left_side_parking ? std::pow(R_E_far, 2) - std::pow(-arc_coordinates.distance / 2 + R_E_far, 2) + : std::pow(R_E_far, 2) - std::pow(arc_coordinates.distance / 2 + R_E_far, 2); if (squared_distance_to_arc_connect < 0) { // may be current_pose is behind the lane return boost::none; @@ -351,8 +363,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( const Pose current_pose = planner_data_->self_odometry->pose.pose; const auto current_arc_position = lanelet::utils::getArcCoordinates(road_lanes, current_pose); - auto path = planner_data_->route_handler->getCenterLinePath( - road_lanes, current_arc_position.length, start_arc_position.length, true); + auto path = utils::resamplePathWithSpline( + planner_data_->route_handler->getCenterLinePath( + road_lanes, current_arc_position.length, start_arc_position.length, true), + parameters_.center_line_path_interval); path.header = planner_data_->route_handler->getRouteHeader(); if (!path.points.empty()) { path.points.back().point.longitudinal_velocity_mps = 0; @@ -362,10 +376,10 @@ PathWithLaneId GeometricParallelParking::generateStraightPath( } std::vector GeometricParallelParking::planOneTrial( - const Pose & start_pose, const Pose & goal_pose, const double R_E_r, + const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const bool is_forward, const double end_pose_offset, const double lane_departure_margin, - const double arc_path_interval) + const bool is_forward, const bool left_side_parking, const double end_pose_offset, + const double lane_departure_margin, const double arc_path_interval) { clearPaths(); @@ -377,18 +391,21 @@ std::vector GeometricParallelParking::planOneTrial( const double goal_yaw = tf2::getYaw(arc_end_pose.orientation); const double psi = normalizeRadian(self_yaw - goal_yaw); - const Pose Cr = calcOffsetPose(arc_end_pose, 0, -R_E_r, 0); - const double d_Cr_Einit = calcDistance2d(Cr, start_pose); + const Pose C_far = left_side_parking ? calcOffsetPose(arc_end_pose, 0, -R_E_far, 0) + : calcOffsetPose(arc_end_pose, 0, R_E_far, 0); + const double d_C_far_Einit = calcDistance2d(C_far, start_pose); - const Point Cr_goal_coords = inverseTransformPoint(Cr.position, arc_end_pose); + const Point C_far_goal_coords = inverseTransformPoint(C_far.position, arc_end_pose); const Point self_point_goal_coords = inverseTransformPoint(start_pose.position, arc_end_pose); const double alpha = - M_PI_2 - psi + std::asin((self_point_goal_coords.y - Cr_goal_coords.y) / d_Cr_Einit); + left_side_parking + ? M_PI_2 - psi + std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit) + : M_PI_2 + psi - std::asin((self_point_goal_coords.y - C_far_goal_coords.y) / d_C_far_Einit); - const double R_E_l = - (std::pow(d_Cr_Einit, 2) - std::pow(R_E_r, 2)) / (2 * (R_E_r + d_Cr_Einit * std::cos(alpha))); - if (R_E_l <= 0) { + const double R_E_near = (std::pow(d_C_far_Einit, 2) - std::pow(R_E_far, 2)) / + (2 * (R_E_far + d_C_far_Einit * std::cos(alpha))); + if (R_E_near <= 0) { return std::vector{}; } @@ -407,48 +424,65 @@ std::vector GeometricParallelParking::planOneTrial( // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle, // and detect lane departure. - if (is_forward) { // Check left bound - const double R_front_left = - std::hypot(R_E_r + common_params.vehicle_width / 2, common_params.base_link2front); - const double distance_to_left_bound = - utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, true); - const double left_deviation = R_front_left - R_E_r; - if (std::abs(distance_to_left_bound) - left_deviation < lane_departure_margin) { + if (is_forward) { // Check near bound + const double R_front_near = + std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front); + const double distance_to_near_bound = + utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking); + const double near_deviation = R_front_near - R_E_far; + if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) { return std::vector{}; } - } else { // Check right bound - const double R_front_right = - std::hypot(R_E_l + common_params.vehicle_width / 2, common_params.base_link2front); - const double right_deviation = R_front_right - R_E_l; - const double distance_to_right_bound = - utils::getSignedDistanceFromBoundary(lanes, start_pose, false); - if (distance_to_right_bound - right_deviation < lane_departure_margin) { + } else { // Check far bound + const double R_front_far = + std::hypot(R_E_near + common_params.vehicle_width / 2, common_params.base_link2front); + const double far_deviation = R_front_far - R_E_near; + const double distance_to_far_bound = + utils::getSignedDistanceFromBoundary(lanes, start_pose, !left_side_parking); + if (std::abs(distance_to_far_bound) - far_deviation < lane_departure_margin) { return std::vector{}; } } - // Generate arc path(left turn -> right turn) - const Pose Cl = calcOffsetPose(start_pose, 0, R_E_l, 0); - double theta_l = std::acos( - (std::pow(R_E_l, 2) + std::pow(R_E_l + R_E_r, 2) - std::pow(d_Cr_Einit, 2)) / - (2 * R_E_l * (R_E_l + R_E_r))); - theta_l = is_forward ? theta_l : -theta_l; - - PathWithLaneId path_turn_left = generateArcPath( - Cl, R_E_l, -M_PI_2, normalizeRadian(-M_PI_2 + theta_l), arc_path_interval, is_forward, - is_forward); - path_turn_left.header = route_handler->getRouteHeader(); + // Generate arc path(first turn -> second turn) + const Pose C_near = left_side_parking ? calcOffsetPose(start_pose, 0, R_E_near, 0) + : calcOffsetPose(start_pose, 0, -R_E_near, 0); + const double theta_near = + std::acos( + (std::pow(R_E_near, 2) + std::pow(R_E_near + R_E_far, 2) - std::pow(d_C_far_Einit, 2)) / + (2 * R_E_near * (R_E_near + R_E_far))) * + (is_forward == left_side_parking ? 1 : -1); + + const auto generateArcPathWithHeader = + [&]( + const auto & C, const auto & R_E, const auto & start_angle, const auto & end_angle, + bool is_forward_first, bool is_forward_second) -> PathWithLaneId { + auto path = generateArcPath( + C, R_E, start_angle, end_angle, arc_path_interval, is_forward_first, is_forward_second); + path.header = route_handler->getRouteHeader(); + return path; + }; - PathWithLaneId path_turn_right = generateArcPath( - Cr, R_E_r, normalizeRadian(psi + M_PI_2 + theta_l), M_PI_2, arc_path_interval, !is_forward, - is_forward); - path_turn_right.header = route_handler->getRouteHeader(); + PathWithLaneId path_turn_first = + left_side_parking + ? generateArcPathWithHeader( + C_near, R_E_near, -M_PI_2, normalizeRadian(-M_PI_2 + theta_near), is_forward, is_forward) + : generateArcPathWithHeader( + C_near, R_E_near, M_PI_2, normalizeRadian(M_PI_2 + theta_near), !is_forward, is_forward); + + PathWithLaneId path_turn_second = + left_side_parking ? generateArcPathWithHeader( + C_far, R_E_far, normalizeRadian(psi + M_PI_2 + theta_near), M_PI_2, + !is_forward, is_forward) + : generateArcPathWithHeader( + C_far, R_E_far, normalizeRadian(psi - M_PI_2 + theta_near), -M_PI_2, + is_forward, is_forward); // Need to add straight path to last right_turning for parking in parallel if (std::abs(end_pose_offset) > 0) { PathPointWithLaneId straight_point{}; straight_point.point.pose = goal_pose; - path_turn_right.points.push_back(straight_point); + path_turn_second.points.push_back(straight_point); } // Populate lane ids for a given path. @@ -466,8 +500,8 @@ std::vector GeometricParallelParking::planOneTrial( } } }; - populateLaneIds(path_turn_left); - populateLaneIds(path_turn_right); + populateLaneIds(path_turn_first); + populateLaneIds(path_turn_second); // Set lane ids to each point in a given path. // It assigns the accumulated lane ids from path_lane_ids to each point's lane_ids member. @@ -476,12 +510,12 @@ std::vector GeometricParallelParking::planOneTrial( p.lane_ids = path_lane_ids; } }; - setLaneIdsToPath(path_turn_left); - setLaneIdsToPath(path_turn_right); + setLaneIdsToPath(path_turn_first); + setLaneIdsToPath(path_turn_second); // generate arc path vector - paths_.push_back(path_turn_left); - paths_.push_back(path_turn_right); + paths_.push_back(path_turn_first); + paths_.push_back(path_turn_second); // set terminal velocity and acceleration(temporary implementation) if (is_forward) { @@ -502,8 +536,8 @@ std::vector GeometricParallelParking::planOneTrial( arc_end_pose_ = arc_end_pose; // debug - Cr_ = Cr; - Cl_ = Cl; + Cr_ = left_side_parking ? C_far : C_near; + Cl_ = left_side_parking ? C_near : C_far; return paths_; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 5bb4e924b6..9322350877 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -17,7 +17,6 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index f350262cf4..a80f7892b0 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -35,7 +35,8 @@ GeometricPullOver::GeometricPullOver( parallel_parking_parameters_{parameters.parallel_parking_parameters}, lane_departure_checker_{lane_departure_checker}, occupancy_grid_map_{occupancy_grid_map}, - is_forward_{is_forward} + is_forward_{is_forward}, + left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { planner_.setParameters(parallel_parking_parameters_); } @@ -63,7 +64,7 @@ boost::optional GeometricPullOver::plan(const Pose & goal_pose) planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_); + planner_.planPullOver(goal_pose, road_lanes, pull_over_lanes, is_forward_, left_side_parking_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index 2dd81eba7e..c1d24f02ac 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -17,12 +17,16 @@ #include "behavior_path_planner/utils/goal_planner/util.hpp" #include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" #include "behavior_path_planner/utils/path_utils.hpp" +#include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" +#include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include #include +#include + #include #include @@ -34,6 +38,35 @@ using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::inverseTransformPose; +// Sort with smaller longitudinal distances taking precedence over smaller lateral distances. +struct SortByLongitudinalDistance +{ + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + const double diff = a.distance_from_original_goal - b.distance_from_original_goal; + constexpr double eps = 0.01; + // If the longitudinal distances are approximately equal, sort based on lateral offset. + if (std::abs(diff) < eps) { + return a.lateral_offset < b.lateral_offset; + } + return a.distance_from_original_goal < b.distance_from_original_goal; + } +}; + +// Sort with the weighted sum of the longitudinal distance and the lateral distance weighted by +// lateral_cost. +struct SortByWeightedDistance +{ + double lateral_cost{0.0}; + explicit SortByWeightedDistance(double cost) : lateral_cost(cost) {} + + bool operator()(const GoalCandidate & a, const GoalCandidate & b) const noexcept + { + return a.distance_from_original_goal + lateral_cost * a.lateral_offset < + b.distance_from_original_goal + lateral_cost * b.lateral_offset; + } +}; + GoalSearcher::GoalSearcher( const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map) @@ -75,10 +108,6 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), parameters_.goal_search_interval); - const auto [shoulder_lane_objects, others] = - utils::path_safety_checker::separateObjectsByLanelets( - *(planner_data_->dynamic_object), pull_over_lanes); - std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; for (const auto & p : center_line_path.points) { @@ -141,8 +170,13 @@ GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) } createAreaPolygons(original_search_poses); - // Sort with distance from original goal - std::sort(goal_candidates.begin(), goal_candidates.end()); + if (parameters_.goal_priority == "minimum_weighted_distance") { + std::sort( + goal_candidates.begin(), goal_candidates.end(), + SortByWeightedDistance(parameters_.minimum_weighted_distance_lateral_weight)); + } else if (parameters_.goal_priority == "minimum_longitudinal_distance") { + std::sort(goal_candidates.begin(), goal_candidates.end(), SortByLongitudinalDistance()); + } return goal_candidates; } @@ -159,16 +193,18 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const continue; } - // check margin with pull over lane objects + // check longitudinal margin with pull over lane objects const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - const auto [shoulder_lane_objects, others] = + const auto [pull_over_lane_objects, others] = utils::path_safety_checker::separateObjectsByLanelets( *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); constexpr bool filter_inside = true; const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance( - goal_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + goal_pose, planner_data_->parameters.vehicle_width, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(goal_pose, target_objects)) { goal_candidate.is_safe = false; @@ -181,7 +217,7 @@ void GoalSearcher::update(GoalCandidates & goal_candidates) const bool GoalSearcher::checkCollision(const Pose & pose) const { - if (parameters_.use_occupancy_grid) { + if (parameters_.use_occupancy_grid_for_goal_search) { const Pose pose_grid_coords = global2local(occupancy_grid_map_->getMap(), pose); const auto idx = pose2index( occupancy_grid_map_->getMap(), pose_grid_coords, occupancy_grid_map_->getParam().theta_size); @@ -192,8 +228,16 @@ bool GoalSearcher::checkCollision(const Pose & pose) const } if (parameters_.use_object_recognition) { + const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); + const auto [pull_over_lane_objects, others] = + utils::path_safety_checker::separateObjectsByLanelets( + *(planner_data_->dynamic_object), pull_over_lanes); + const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity( + pull_over_lane_objects, parameters_.th_moving_object_velocity); if (utils::checkCollisionBetweenFootprintAndObjects( - vehicle_footprint_, pose, *(planner_data_->dynamic_object), + vehicle_footprint_, pose, pull_over_lane_stop_objects, parameters_.object_recognition_collision_check_margin)) { return true; } @@ -204,7 +248,9 @@ bool GoalSearcher::checkCollision(const Pose & pose) const bool GoalSearcher::checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const { - if (parameters_.use_occupancy_grid && parameters_.use_occupancy_grid_for_longitudinal_margin) { + if ( + parameters_.use_occupancy_grid_for_goal_search && + parameters_.use_occupancy_grid_for_goal_longitudinal_margin) { constexpr bool check_out_of_range = false; const double offset = std::max( parameters_.longitudinal_margin - parameters_.occupancy_grid_collision_check_margin, 0.0); diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index d6a3746187..e1bdaf977d 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -112,7 +112,7 @@ boost::optional ShiftPullOver::generatePullOverPath( // generate road lane reference path to shift end const auto road_lane_reference_path_to_shift_end = utils::resamplePathWithSpline( - generateReferencePath(road_lanes, shift_end_pose), resample_interval_); + generateReferencePath(road_lanes, shift_end_pose), parameters_.center_line_path_interval); // calculate shift length const Pose & shift_end_pose_road_lane = @@ -172,9 +172,6 @@ boost::optional ShiftPullOver::generatePullOverPath( shifted_path.path.points.push_back(p); } - shifted_path.path = - utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); - // set the same z as the goal for (auto & p : shifted_path.path.points) { p.point.pose.position.z = goal_pose.position.z; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp index aa3380c2b9..4f24d1b682 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/util.cpp @@ -20,11 +20,11 @@ #include #include #include -#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index ffd5116a5d..e4fbf41e5a 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -22,7 +22,6 @@ #include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/utils.hpp" -#include #include #include #include @@ -32,6 +31,9 @@ #include #include +#include +#include +#include #include #include @@ -655,22 +657,6 @@ lanelet::ConstLanelets getBackwardLanelets( return backward_lanes; } -bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters) -{ - using autoware_auto_perception_msgs::msg::ObjectClassification; - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && parameters.check_car) || - (t == ObjectClassification::TRUCK && parameters.check_truck) || - (t == ObjectClassification::BUS && parameters.check_bus) || - (t == ObjectClassification::TRAILER && parameters.check_trailer) || - (t == ObjectClassification::UNKNOWN && parameters.check_unknown) || - (t == ObjectClassification::BICYCLE && parameters.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && parameters.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && parameters.check_pedestrian)); - return is_object_type; -} - double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer) { return lateral_buffer + 0.5 * vehicle_width; @@ -827,19 +813,6 @@ std::vector convertToPredictedPath( return predicted_path; } -PredictedPath convertToPredictedPath( - const std::vector & path, const double time_resolution) -{ - PredictedPath predicted_path; - predicted_path.time_step = rclcpp::Duration::from_seconds(time_resolution); - predicted_path.path.resize(path.size()); - - for (size_t i = 0; i < path.size(); ++i) { - predicted_path.path.at(i) = path.at(i).pose; - } - return predicted_path; -} - bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, const ExtendedPredictedObject & object, const double object_check_min_road_shoulder_width, diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index 7205b273ff..ff54ce2d2d 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -18,7 +18,8 @@ #include #include -#include + +#include namespace behavior_path_planner::utils::path_safety_checker { @@ -111,21 +112,10 @@ void filterObjectsByPosition( void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; - PredictedObjects filtered_objects; for (auto & object : objects.objects) { - const auto t = utils::getHighestProbLabel(object.classification); - const auto is_object_type = - ((t == ObjectClassification::CAR && target_object_types.check_car) || - (t == ObjectClassification::TRUCK && target_object_types.check_truck) || - (t == ObjectClassification::BUS && target_object_types.check_bus) || - (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || - (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || - (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || - (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || - (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); + const auto is_object_type = isTargetObjectType(object, target_object_types); // If the object type matches any of the target types, add it to the filtered list if (is_object_type) { @@ -135,8 +125,6 @@ void filterObjectsByClass( // Replace the original objects with the filtered list objects = std::move(filtered_objects); - - return; } std::pair, std::vector> separateObjectIndicesByLanelets( @@ -221,28 +209,44 @@ std::vector getPredictedPathFromObj( return obj.predicted_paths; } -// TODO(Sugahara): should consider delay before departure std::vector createPredictedPath( const std::shared_ptr & ego_predicted_path_params, const std::vector & path_points, - const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, size_t ego_seg_idx) + const geometry_msgs::msg::Pose & vehicle_pose, const double current_velocity, + const size_t ego_seg_idx, const bool is_object_front, const bool limit_to_max_velocity) { if (path_points.empty()) { return {}; } - const double min_slow_down_speed = ego_predicted_path_params->min_slow_speed; + const double min_velocity = ego_predicted_path_params->min_velocity; const double acceleration = ego_predicted_path_params->acceleration; - const double time_horizon = ego_predicted_path_params->time_horizon; + const double max_velocity = limit_to_max_velocity ? ego_predicted_path_params->max_velocity + : std::numeric_limits::infinity(); + const double time_horizon = is_object_front + ? ego_predicted_path_params->time_horizon_for_front_object + : ego_predicted_path_params->time_horizon_for_rear_object; const double time_resolution = ego_predicted_path_params->time_resolution; + const double delay_until_departure = ego_predicted_path_params->delay_until_departure; std::vector predicted_path; const auto vehicle_pose_frenet = convertToFrenetPoint(path_points, vehicle_pose.position, ego_seg_idx); - for (double t = 0.0; t < time_horizon + 1e-3; t += time_resolution) { - const double velocity = std::max(current_velocity + acceleration * t, min_slow_down_speed); - const double length = current_velocity * t + 0.5 * acceleration * t * t; + for (double t = 0.0; t < time_horizon; t += time_resolution) { + double velocity = 0.0; + double length = 0.0; + + // If t < delay_until_departure, it means ego have not depart yet, therefore the velocity is + // 0 and there's no change in position. + if (t >= delay_until_departure) { + // Adjust time to consider the delay. + double t_with_delay = t - delay_until_departure; + velocity = + std::clamp(current_velocity + acceleration * t_with_delay, min_velocity, max_velocity); + length = current_velocity * t_with_delay + 0.5 * acceleration * t_with_delay * t_with_delay; + } + const auto pose = motion_utils::calcInterpolatedPose(path_points, vehicle_pose_frenet.length + length); predicted_path.emplace_back(t, pose, velocity); @@ -360,4 +364,19 @@ TargetObjectsOnLane createTargetObjectsOnLane( return target_objects_on_lane; } +bool isTargetObjectType( + const PredictedObject & object, const ObjectTypesToCheck & target_object_types) +{ + using autoware_auto_perception_msgs::msg::ObjectClassification; + const auto t = utils::getHighestProbLabel(object.classification); + return ( + (t == ObjectClassification::CAR && target_object_types.check_car) || + (t == ObjectClassification::TRUCK && target_object_types.check_truck) || + (t == ObjectClassification::BUS && target_object_types.check_bus) || + (t == ObjectClassification::TRAILER && target_object_types.check_trailer) || + (t == ObjectClassification::UNKNOWN && target_object_types.check_unknown) || + (t == ObjectClassification::BICYCLE && target_object_types.check_bicycle) || + (t == ObjectClassification::MOTORCYCLE && target_object_types.check_motorcycle) || + (t == ObjectClassification::PEDESTRIAN && target_object_types.check_pedestrian)); +} } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp index dddcc672e7..9fb7d0acdf 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp @@ -21,6 +21,10 @@ #include "object_recognition_utils/predicted_path_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include +#include +#include + namespace behavior_path_planner::utils::path_safety_checker { void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) @@ -32,6 +36,12 @@ void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & bg::append(polygon.outer(), point); } +bool isTargetObjectOncoming( + const geometry_msgs::msg::Pose & vehicle_pose, const geometry_msgs::msg::Pose & object_pose) +{ + return std::abs(calcYawDeviation(vehicle_pose, object_pose)) > M_PI_2; +} + bool isTargetObjectFront( const geometry_msgs::msg::Pose & ego_pose, const Polygon2d & obj_polygon, const vehicle_info_util::VehicleInfo & vehicle_info) diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 44ba6bd364..21d4ff666e 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -56,6 +56,7 @@ namespace behavior_path_planner using motion_utils::findNearestIndex; using motion_utils::insertOrientation; using motion_utils::removeInvalidOrientationPoints; +using motion_utils::removeOverlapPoints; void PathShifter::setPath(const PathWithLaneId & path) { @@ -147,9 +148,18 @@ bool PathShifter::generate( type == SHIFT_TYPE::SPLINE ? applySplineShifter(shifted_path, offset_back) : applyLinearShifter(shifted_path); - const bool is_driving_forward = true; - insertOrientation(shifted_path->path.points, is_driving_forward); + shifted_path->path.points = removeOverlapPoints(shifted_path->path.points); + // Use orientation before shift to remove points in reverse order + // before setting wrong azimuth orientation removeInvalidOrientationPoints(shifted_path->path.points); + size_t previous_size{shifted_path->path.points.size()}; + do { + previous_size = shifted_path->path.points.size(); + // Set the azimuth orientation to the next point at each point + insertOrientation(shifted_path->path.points, true); + // Use azimuth orientation to remove points in reverse order + removeInvalidOrientationPoints(shifted_path->path.points); + } while (previous_size != shifted_path->path.points.size()); // DEBUG RCLCPP_DEBUG_STREAM_THROTTLE( diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner/src/utils/path_utils.cpp index 112c7e49c7..7673c9a1de 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner/src/utils/path_utils.cpp @@ -23,6 +23,7 @@ #include #include +// #include #include #include diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index bd695055b2..1eaea06c02 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -14,8 +14,6 @@ #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include - namespace behavior_path_planner::utils::start_goal_planner_common { @@ -140,7 +138,7 @@ void updatePathProperty( const double acceleration = std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); - ego_predicted_path_params->target_velocity = pairs_terminal_velocity_and_accel.first; + ego_predicted_path_params->max_velocity = pairs_terminal_velocity_and_accel.first; ego_predicted_path_params->acceleration = acceleration; } @@ -169,19 +167,36 @@ std::pair getPairsTerminalVelocityAndAccel( return pairs_terminal_velocity_and_accel.at(current_path_idx); } -PathWithLaneId removeInverseOrderPathPoints(const PathWithLaneId & path) +std::optional generateFeasibleStopPath( + PathWithLaneId & current_path, std::shared_ptr planner_data, + geometry_msgs::msg::Pose & stop_pose, const double maximum_deceleration, + const double maximum_jerk) { - PathWithLaneId fixed_path; - fixed_path.header = path.header; - fixed_path.points.push_back(path.points.at(0)); - for (size_t i = 1; i < path.points.size(); i++) { - const auto p1 = path.points.at(i - 1).point.pose; - const auto p2 = path.points.at(i).point.pose; - if (tier4_autoware_utils::isDrivingForward(p1, p2)) { - fixed_path.points.push_back(path.points.at(i)); - } + if (current_path.points.empty()) { + return {}; + } + + // try to insert stop point in current_path after approval + // but if can't stop with constraints(maximum deceleration, maximum jerk), don't insert stop point + const auto min_stop_distance = + behavior_path_planner::utils::start_goal_planner_common::calcFeasibleDecelDistance( + planner_data, maximum_deceleration, maximum_jerk, 0.0); + + if (!min_stop_distance) { + return {}; } - return fixed_path; + + // set stop point + const auto stop_idx = motion_utils::insertStopPoint( + planner_data->self_odometry->pose.pose, *min_stop_distance, current_path.points); + + if (!stop_idx) { + return {}; + } + + stop_pose = current_path.points.at(*stop_idx).point.pose; + + return current_path; } } // namespace behavior_path_planner::utils::start_goal_planner_common diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index 36fb6381ac..8e5d6b7545 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -96,7 +96,9 @@ boost::optional FreespacePullOut::plan(const Pose & start_pose, con const bool path_terminal_is_goal = path_end_info.second; const auto road_center_line_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - last_path = start_planner_utils::combineReferencePath(last_path, road_center_line_path); + last_path = utils::resamplePathWithSpline( + start_planner_utils::combineReferencePath(last_path, road_center_line_path), + parameters_.center_line_path_interval); const double original_terminal_velocity = last_path.points.back().point.longitudinal_velocity_mps; utils::correctDividedPathVelocity(partial_paths); diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 0d75391f9d..5472885359 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -19,7 +19,6 @@ #include "behavior_path_planner/utils/utils.hpp" #include -#include using lanelet::utils::getArcCoordinates; using motion_utils::findNearestIndex; @@ -49,11 +48,14 @@ boost::optional GeometricPullOut::plan(const Pose & start_pose, con /*forward_only_in_route*/ true); const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); + // check if the ego is at left or right side of road lane center + const bool left_side_start = 0 < getArcCoordinates(road_lanes, start_pose).distance; + planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); planner_.setPlannerData(planner_data_); const bool found_valid_path = - planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes); + planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index 6225ce598a..2de72d7819 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -59,8 +59,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P planner_data_, backward_path_length, std::numeric_limits::max(), /*forward_only_in_route*/ true); // find candidate paths - auto pull_out_paths = calcPullOutPaths( - *route_handler, road_lanes, start_pose, goal_pose, common_parameters, parameters_); + auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { return boost::none; } @@ -160,8 +159,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P std::vector ShiftPullOut::calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, - const Pose & start_pose, const Pose & goal_pose, - const BehaviorPathPlannerParameters & common_parameter, const StartPlannerParameters & parameter) + const Pose & start_pose, const Pose & goal_pose) { std::vector candidate_paths{}; @@ -170,12 +168,16 @@ std::vector ShiftPullOut::calcPullOutPaths( } // rename parameter - const double forward_path_length = common_parameter.forward_path_length; - const double backward_path_length = common_parameter.backward_path_length; - const double lateral_jerk = parameter.lateral_jerk; - const double minimum_lateral_acc = parameter.minimum_lateral_acc; - const double maximum_lateral_acc = parameter.maximum_lateral_acc; - const int lateral_acceleration_sampling_num = parameter.lateral_acceleration_sampling_num; + const auto & common_parameters = planner_data_->parameters; + const double forward_path_length = common_parameters.forward_path_length; + const double backward_path_length = common_parameters.backward_path_length; + const double lateral_jerk = parameters_.lateral_jerk; + const double minimum_lateral_acc = parameters_.minimum_lateral_acc; + const double maximum_lateral_acc = parameters_.maximum_lateral_acc; + const double maximum_curvature = parameters_.maximum_curvature; + const double minimum_shift_pull_out_distance = parameters_.minimum_shift_pull_out_distance; + const int lateral_acceleration_sampling_num = parameters_.lateral_acceleration_sampling_num; + // set minimum acc for breaking loop when sampling num is 1 const double acc_resolution = std::max( std::abs(maximum_lateral_acc - minimum_lateral_acc) / lateral_acceleration_sampling_num, @@ -189,9 +191,9 @@ std::vector ShiftPullOut::calcPullOutPaths( const double s_end = path_end_info.first; const bool path_terminal_is_goal = path_end_info.second; - constexpr double RESAMPLE_INTERVAL = 1.0; PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline( - route_handler.getCenterLinePath(road_lanes, s_start, s_end), RESAMPLE_INTERVAL); + route_handler.getCenterLinePath(road_lanes, s_start, s_end), + parameters_.center_line_path_interval); // non_shifted_path for when shift length or pull out distance is too short const PullOutPath non_shifted_path = std::invoke([&]() { @@ -230,8 +232,8 @@ std::vector ShiftPullOut::calcPullOutPaths( PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, /* max acc */ 1.0); const auto pull_out_distance = calcPullOutLongitudinalDistance( - longitudinal_acc, shift_time, shift_length, parameter.maximum_curvature, - parameter.minimum_shift_pull_out_distance); + longitudinal_acc, shift_time, shift_length, maximum_curvature, + minimum_shift_pull_out_distance); const double terminal_velocity = longitudinal_acc * shift_time; // clip from ego pose @@ -253,7 +255,9 @@ std::vector ShiftPullOut::calcPullOutPaths( std::max(pull_out_distance, pull_out_distance_converted); // if before_shifted_pull_out_distance is too short, shifting path fails, so add non shifted - if (before_shifted_pull_out_distance < RESAMPLE_INTERVAL && !has_non_shifted_path) { + if ( + before_shifted_pull_out_distance < parameters_.center_line_path_interval && + !has_non_shifted_path) { candidate_paths.push_back(non_shifted_path); has_non_shifted_path = true; continue; @@ -299,9 +303,6 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } - shifted_path.path = - utils::start_goal_planner_common::removeInverseOrderPathPoints(shifted_path.path); - // set velocity const size_t pull_out_end_idx = findNearestIndex(shifted_path.path.points, shift_end_pose_ptr->position); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index d6af88f39a..1b14741db4 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -27,7 +27,11 @@ #include "autoware_auto_perception_msgs/msg/predicted_object.hpp" #include "autoware_auto_perception_msgs/msg/predicted_path.hpp" -#include +#include + +#include +#include +#include #include #include @@ -1540,7 +1544,9 @@ void generateDrivableArea( // make bound longitudinally monotonic // TODO(Murooka) Fix makeBoundLongitudinallyMonotonic - if (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas) { + if ( + is_driving_forward && + (enable_expanding_hatched_road_markings || enable_expanding_intersection_areas)) { makeBoundLongitudinallyMonotonic(path, planner_data, true); // for left bound makeBoundLongitudinallyMonotonic(path, planner_data, false); // for right bound } @@ -1669,6 +1675,13 @@ std::vector calcBound( continue; } + if (!enable_expanding_hatched_road_markings) { + for (const auto & point : bound) { + output_points.push_back(lanelet::utils::conversion::toGeomMsgPt(point)); + } + continue; + } + // expand drivable area by hatched road markings. for (size_t bound_point_idx = 0; bound_point_idx < bound.size(); ++bound_point_idx) { const auto & bound_point = bound[bound_point_idx]; @@ -2436,21 +2449,16 @@ std::optional getSignedDistanceFromBoundary( const auto & bound_line_2d = left_side ? lanelet::utils::to2D(combined_lane.leftBound3d()) : lanelet::utils::to2D(combined_lane.rightBound3d()); - // Initialize the lateral distance to the maximum (for left side) or the minimum (for right side) - // possible value. - double lateral_distance = - left_side ? -std::numeric_limits::max() : std::numeric_limits::max(); - // Find the closest bound segment that contains the corner point in the X-direction // and calculate the lateral distance from that segment. - const auto calcLateralDistanceFromBound = [&]( - const Point & vehicle_corner_point, - boost::optional & lateral_distance, - boost::optional & segment_idx) { + const auto calcLateralDistanceFromBound = + [&](const Point & vehicle_corner_point) -> boost::optional> { Pose vehicle_corner_pose{}; vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; + boost::optional> lateral_distance_with_idx{}; + // Euclidean distance to find the closest segment containing the corner point. double min_distance = std::numeric_limits::max(); @@ -2476,38 +2484,45 @@ std::optional getSignedDistanceFromBoundary( min_distance = std::min(distance1, distance2); // Update lateral distance using the formula derived from similar triangles in the lateral // cross-section view. - lateral_distance = -1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1); - segment_idx = i; + lateral_distance_with_idx = + std::make_pair(-1.0 * (dy_p1 * dx_p2 + dy_p2 * -dx_p1) / (dx_p2 - dx_p1), i); } } + if (lateral_distance_with_idx) { + return lateral_distance_with_idx; + } + return boost::optional>{}; }; // Calculate the lateral distance for both the rear and front corners of the vehicle. - boost::optional rear_segment_idx{}; - boost::optional rear_lateral_distance{}; - calcLateralDistanceFromBound(rear_corner_point, rear_lateral_distance, rear_segment_idx); - boost::optional front_segment_idx{}; - boost::optional front_lateral_distance{}; - calcLateralDistanceFromBound(front_corner_point, front_lateral_distance, front_segment_idx); + const boost::optional> rear_lateral_distance_with_idx = + calcLateralDistanceFromBound(rear_corner_point); + const boost::optional> front_lateral_distance_with_idx = + calcLateralDistanceFromBound(front_corner_point); // If no closest bound segment was found for both corners, return an empty optional. - if (!rear_lateral_distance && !front_lateral_distance) { + if (!rear_lateral_distance_with_idx && !front_lateral_distance_with_idx) { return {}; } // If only one of them found the closest bound, return the found lateral distance. - if (!rear_lateral_distance) { - return *front_lateral_distance; - } else if (!front_lateral_distance) { - return *rear_lateral_distance; + if (!rear_lateral_distance_with_idx) { + return front_lateral_distance_with_idx.get().first; + } else if (!front_lateral_distance_with_idx) { + return rear_lateral_distance_with_idx.get().first; } // If both corners found their closest bound, return the maximum (for left side) or the minimum // (for right side) lateral distance. - lateral_distance = left_side ? std::max(*rear_lateral_distance, *front_lateral_distance) - : std::min(*rear_lateral_distance, *front_lateral_distance); + double lateral_distance = + left_side + ? std::max( + rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first) + : std::min( + rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first); // Iterate through all segments between the segments closest to the rear and front corners. // Update the lateral distance in case any of these inner segments are closer to the vehicle. - for (size_t i = *rear_segment_idx + 1; i < *front_segment_idx; i++) { + for (size_t i = rear_lateral_distance_with_idx.get().second + 1; + i < front_lateral_distance_with_idx.get().second; i++) { Pose bound_pose; bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); bound_pose.orientation = vehicle_pose.orientation; @@ -3141,15 +3156,12 @@ double calcMinimumLaneChangeLength( const double & max_lateral_acc = lat_acc.second; const double & lateral_jerk = common_param.lane_changing_lateral_jerk; const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const auto prepare_length = 0.5 * common_param.max_acc * - common_param.lane_change_prepare_duration * - common_param.lane_change_prepare_duration; double accumulated_length = length_to_intersection; for (const auto & shift_interval : shift_intervals) { const double t = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + prepare_length + finish_judge_buffer; + accumulated_length += vel * t + finish_judge_buffer; } accumulated_length += common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp index 7a5bb68dec..e0a16089b8 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp @@ -20,6 +20,7 @@ #include "lanelet2_extension/utility/message_conversion.hpp" #include +#include using drivable_area_expansion::linestring_t; using drivable_area_expansion::point_t; diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner/test/test_turn_signal.cpp index 4917443064..454be6b75f 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner/test/test_turn_signal.cpp @@ -12,11 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. #include "behavior_path_planner/turn_signal_decider.hpp" -#include "motion_utils/trajectory/path_with_lane_id.hpp" +#include "motion_utils/trajectory/trajectory.hpp" -#include #include -#include #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index 1452e45980..4eefa27e88 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/utils.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" +#include "lanelet2_core/geometry/LineString.h" #include "lanelet2_core/geometry/Point.h" #include "lanelet2_core/primitives/Lanelet.h" #include "motion_utils/trajectory/path_with_lane_id.hpp" diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 1652d9d56a..f95ed73c2a 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 440c360e71..775f3b1c65 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -20,7 +20,7 @@ #include #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index dfe682d0aa..6c70e7a7f6 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -43,8 +43,6 @@ namespace behavior_velocity_planner using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using tier4_autoware_utils::createPoint; -using tier4_autoware_utils::Point2d; using tier4_planning_msgs::msg::StopFactor; enum class CollisionState { YIELD, EGO_PASS_FIRST, EGO_PASS_LATER, IGNORE }; diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index 5a9de20d79..304b4bdf6f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -125,15 +125,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - if (!opt_use_regulatory_element_) { - opt_use_regulatory_element_ = checkRegulatoryElementExistence(rh->getLaneletMapPtr()); - std::ostringstream string_stream; - string_stream << "use crosswalk regulatory element: "; - string_stream << std::boolalpha << *opt_use_regulatory_element_; - RCLCPP_INFO_STREAM(logger_, string_stream.str()); - } - const auto launch = [this, &path](const auto id) { + const auto launch = [this, &path](const auto id, const auto & use_regulatory_element) { if (isModuleRegistered(id)) { return; } @@ -143,26 +136,23 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto lanelet_map_ptr = planner_data_->route_handler_->getLaneletMapPtr(); registerModule(std::make_shared( - node_, id, lanelet_map_ptr, p, *opt_use_regulatory_element_, logger, clock_)); + node_, id, lanelet_map_ptr, p, use_regulatory_element, logger, clock_)); generateUUID(id); updateRTCStatus(getUUID(id), true, std::numeric_limits::lowest(), path.header.stamp); }; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - launch(crosswalk.first->id()); - } - } else { - const auto crosswalk_lanelets = getCrosswalksOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + for (const auto & crosswalk : crosswalk_leg_elem_map) { + launch(crosswalk.first->id(), true); + } - for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id()); - } + const auto crosswalk_lanelets = getCrosswalksOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); + + for (const auto & crosswalk : crosswalk_lanelets) { + launch(crosswalk.id(), false); } } @@ -173,17 +163,14 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) std::set crosswalk_id_set; - if (*opt_use_regulatory_element_) { - const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( - path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + crosswalk_id_set = getCrosswalkIdSetOnPath( + planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); - for (const auto & crosswalk : crosswalk_leg_elem_map) { - crosswalk_id_set.insert(crosswalk.first->id()); - } - } else { - crosswalk_id_set = getCrosswalkIdSetOnPath( - planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), - rh->getOverallGraphPtr()); + const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( + path, rh->getLaneletMapPtr(), planner_data_->current_odometry->pose); + + for (const auto & crosswalk : crosswalk_leg_elem_map) { + crosswalk_id_set.insert(crosswalk.first->id()); } return [crosswalk_id_set](const std::shared_ptr & scene_module) { diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index 481a246af8..38bb63c121 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -51,8 +51,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class CrosswalkModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index ab84caa782..9ea03996c8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -19,11 +19,15 @@ #include #include #include +#include #include #include #include #include +#include +#include + #include #include #include diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 11e2d600c6..667298cbb4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -19,8 +19,6 @@ #include #include -#include -#include #include #include #include @@ -32,9 +30,8 @@ #include #include -#include -#include -#include +#include +#include #include #include #include diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 6fabc4c201..47bbef7111 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -15,7 +15,9 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include +#include #include +#include #include #include @@ -27,6 +29,8 @@ #include #include +#include + #include #include #include @@ -37,9 +41,8 @@ #include #include #include -#include -#include +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index b3d3d5c781..ee2af54e5e 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #ifdef ROS_DISTRO_GALACTIC diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index 16d32b8a49..e7ec6b37f7 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "manager.hpp" +#include #include #include diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp index 53a6deb6f2..4d635d3d6d 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.cpp @@ -17,13 +17,14 @@ #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include + #include #include #include diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index c30c5d70f8..a932254140 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -2,7 +2,7 @@ ros__parameters: intersection: common: - attention_area_margin: 0.5 # [m] + attention_area_margin: 0.75 # [m] attention_area_length: 200.0 # [m] attention_area_angle_threshold: 0.785 # [rad] stop_line_margin: 3.0 @@ -14,7 +14,7 @@ consider_wrong_direction_vehicle: false stuck_vehicle: use_stuck_stopline: true # stopline generated before the first conflicting area - 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_detect_dist: 5.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_vel_thr: 0.833 # 0.833m/s = 3.0km/h # enable_front_car_decel_prediction: false # By default this feature is disabled # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning @@ -24,12 +24,15 @@ state_transit_margin_time: 1.0 min_predicted_path_confidence: 0.05 minimum_ego_predicted_velocity: 1.388 # [m/s] - normal: - collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - relaxed: + fully_prioritized: collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 + partially_prioritized: + collision_start_margin_time: 2.0 + collision_end_margin_time: 2.0 + not_prioritized: + collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object + collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr occlusion: @@ -45,14 +48,16 @@ min_vehicle_brake_for_rss: -2.5 # [m/s^2] max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph denoise_kernel: 1.0 # [m] - possible_object_bbox: [1.0, 2.0] # [m x m] - ignore_parked_vehicle_speed_threshold: 0.333 # == 1.2km/h - stop_release_margin_time: 1.0 # [s] + possible_object_bbox: [1.5, 2.5] # [m x m] + ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h + stop_release_margin_time: 1.5 # [s] + temporal_stop_before_attention_area: false - enable_rtc: # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval - intersection: true + enable_rtc: + intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval intersection_to_occlusion: true merge_from_private: stop_line_margin: 3.0 stop_duration_sec: 1.0 + stop_distance_threshold: 1.0 diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index dfaa10ec9d..af8972424f 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -20,6 +20,14 @@ #include #include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + #include #include @@ -105,7 +113,33 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( return msg; } -visualization_msgs::msg::Marker createPointMarkerArray( +visualization_msgs::msg::MarkerArray createLineMarkerArray( + const geometry_msgs::msg::Point & point_start, const geometry_msgs::msg::Point & point_end, + const std::string & ns, const int64_t id, const double r, const double g, const double b) +{ + visualization_msgs::msg::MarkerArray msg; + + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.ns = ns + "_line"; + marker.id = id; + marker.lifetime = rclcpp::Duration::from_seconds(0.3); + marker.type = visualization_msgs::msg::Marker::ARROW; + marker.action = visualization_msgs::msg::Marker::ADD; + geometry_msgs::msg::Vector3 arrow; + arrow.x = 1.0; + arrow.y = 1.0; + arrow.z = 1.0; + marker.scale = arrow; + marker.color = createMarkerColor(r, g, b, 0.999); + marker.points.push_back(point_start); + marker.points.push_back(point_end); + + msg.markers.push_back(marker); + return msg; +} + +[[maybe_unused]] visualization_msgs::msg::Marker createPointMarkerArray( const geometry_msgs::msg::Point & point, const std::string & ns, const int64_t id, const double r, const double g, const double b) { @@ -177,17 +211,6 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } - if (debug_data_.nearest_occlusion_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_point.value(), "nearest_occlusion", module_id_, 0.5, 0.5, 0.0)); - } - - if (debug_data_.nearest_occlusion_projection_point) { - debug_marker_array.markers.push_back(createPointMarkerArray( - debug_data_.nearest_occlusion_projection_point.value(), "nearest_occlusion_projection", - module_id_, 0.5, 0.5, 0.0)); - } - size_t i{0}; for (const auto & p : debug_data_.candidate_collision_object_polygons) { appendMarkerArray( @@ -230,6 +253,13 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); } + if (debug_data_.nearest_occlusion_projection) { + const auto [point_start, point_end] = debug_data_.nearest_occlusion_projection.value(); + appendMarkerArray( + createLineMarkerArray( + point_start, point_end, "nearest_occlusion_projection", lane_id_, 0.5, 0.5, 0.0), + &debug_marker_array, now); + } return debug_marker_array; } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index bf0970f364..28cc7a1ad2 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -84,14 +84,23 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); ip.collision_detection.state_transit_margin_time = getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); - ip.collision_detection.normal.collision_start_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.normal.collision_start_margin_time"); - ip.collision_detection.normal.collision_end_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.normal.collision_end_margin_time"); - ip.collision_detection.relaxed.collision_start_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.relaxed.collision_start_margin_time"); - ip.collision_detection.relaxed.collision_end_margin_time = getOrDeclareParameter( - node, ns + ".collision_detection.relaxed.collision_end_margin_time"); + ip.collision_detection.fully_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); + ip.collision_detection.fully_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.fully_prioritized.collision_end_margin_time"); + ip.collision_detection.partially_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_start_margin_time"); + ip.collision_detection.partially_prioritized.collision_end_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.partially_prioritized.collision_end_margin_time"); + ip.collision_detection.not_prioritized.collision_start_margin_time = + getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); + ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter( + node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); ip.collision_detection.keep_detection_vel_thr = getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); @@ -121,6 +130,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); ip.occlusion.stop_release_margin_time = getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); + ip.occlusion.temporal_stop_before_attention_area = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); } void IntersectionModuleManager::launchNewModules( @@ -273,6 +284,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node mp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); + mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); } void MergeFromPrivateModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 5f9d838b7c..fec7589f3c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -19,16 +19,19 @@ #include #include #include -#include -#include -#include #include -#include +#include #include #include +#include +#include +#include +#include + +#include #include -#include +#include #include #include @@ -95,6 +98,11 @@ IntersectionModule::IntersectionModule( stuck_private_area_timeout_.setMarginTime(planner_param_.stuck_vehicle.timeout_private_area); stuck_private_area_timeout_.setState(StateMachine::State::STOP); } + { + temporal_stop_before_attention_state_machine_.setMarginTime( + planner_param_.occlusion.before_creep_stop_time); + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); + } decision_state_pub_ = node_.create_publisher("~/debug/intersection/decision_state", 1); @@ -477,7 +485,10 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; + const size_t occlusion_peeking_stop_line = + decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; if (planner_param.occlusion.enable_creeping) { const size_t closest_idx = decision_result.closest_idx; for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { @@ -541,7 +552,9 @@ void reactRTCApprovalByDecisionResult( } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; + const auto stop_line_idx = decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stop_line_idx + : decision_result.occlusion_stop_line_idx; planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); @@ -753,9 +766,11 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_param_.occlusion.occlusion_attention_area_length, planner_param_.common.consider_wrong_direction_vehicle); } - const bool tl_arrow_solid_on = - util::isTrafficLightArrowActivated(assigned_lanelet, planner_data_->traffic_light_id_map); - intersection_lanelets_.value().update(tl_arrow_solid_on, interpolated_path_info); + const auto traffic_prioritized_level = + util::getTrafficPrioritizedLevel(assigned_lanelet, planner_data_->traffic_light_id_map); + const bool is_prioritized = + traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED; + intersection_lanelets_.value().update(is_prioritized, interpolated_path_info); const auto & conflicting_lanelets = intersection_lanelets_.value().conflicting(); const auto & first_conflicting_area_opt = intersection_lanelets_.value().first_conflicting_area(); @@ -779,7 +794,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); const auto [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, - occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = intersection_stop_lines; + first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, pass_judge_line_idx] = + intersection_stop_lines; const auto & conflicting_area = intersection_lanelets_.value().conflicting_area(); const auto path_lanelets_opt = util::generatePathLanelets( @@ -861,12 +877,13 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( return IntersectionModule::Indecisive{}; } - if (!occlusion_peeking_stop_line_idx_opt) { + if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { RCLCPP_DEBUG(logger_, "occlusion stop line is null"); return IntersectionModule::Indecisive{}; } const auto collision_stop_line_idx = is_over_default_stop_line ? closest_idx : default_stop_line_idx; + const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); const auto & attention_lanelets = intersection_lanelets_.value().attention(); @@ -886,7 +903,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } // calculate dynamic collision around detection area - const double time_delay = (is_go_out_ || tl_arrow_solid_on) + const double time_delay = (is_go_out_ || is_prioritized) ? 0.0 : (planner_param_.collision_detection.state_transit_margin_time - collision_state_machine_.getDuration()); @@ -894,14 +911,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( filterTargetObjects(attention_lanelets, adjacent_lanelets, intersection_area); const bool has_collision = checkCollision( - *path, target_objects, path_lanelets, closest_idx, time_delay, tl_arrow_solid_on); + *path, target_objects, path_lanelets, closest_idx, time_delay, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); const bool has_collision_with_margin = collision_state_machine_.getState() == StateMachine::State::STOP; - if (tl_arrow_solid_on) { + if (is_prioritized) { return TrafficLightArrowSolidOn{ has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; } @@ -927,7 +944,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( object.kinematics.initial_twist_with_covariance.twist.linear.y) <= thresh; }); const bool is_occlusion_cleared = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !tl_arrow_solid_on) + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) ? isOcclusionCleared( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, @@ -944,35 +961,62 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( if ( occlusion_stop_state_machine_.getState() == StateMachine::State::STOP || ext_occlusion_requested) { - const double dist_stopline = motion_utils::calcSignedArcLength( + const double dist_default_stopline = motion_utils::calcSignedArcLength( path->points, path->points.at(closest_idx).point.pose.position, path->points.at(default_stop_line_idx).point.pose.position); - const bool approached_stop_line = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_stop_line = (dist_stopline < 0.0); - const bool is_stopped = + const bool approached_default_stop_line = + (std::fabs(dist_default_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_default_stop_line = (dist_default_stopline < 0.0); + const bool is_stopped_at_default = planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); - if (over_stop_line) { + if (over_default_stop_line) { before_creep_state_machine_.setState(StateMachine::State::GO); } if (before_creep_state_machine_.getState() == StateMachine::State::GO) { + const double dist_first_attention_stopline = motion_utils::calcSignedArcLength( + path->points, path->points.at(closest_idx).point.pose.position, + path->points.at(first_attention_stop_line_idx).point.pose.position); + const bool approached_first_attention_stop_line = + (std::fabs(dist_first_attention_stopline) < planner_param_.common.stop_overshoot_margin); + const bool over_first_attention_stop_line = (dist_first_attention_stopline < 0.0); + const bool is_stopped_at_first_attention = + planner_data_->isVehicleStopped(planner_param_.occlusion.before_creep_stop_time); + if (planner_param_.occlusion.temporal_stop_before_attention_area) { + if (over_first_attention_stop_line) { + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); + } + if (is_stopped_at_first_attention && approached_first_attention_stop_line) { + temporal_stop_before_attention_state_machine_.setState(StateMachine::State::GO); + } + } + const bool temporal_stop_before_attention_required = + planner_param_.occlusion.temporal_stop_before_attention_area && + temporal_stop_before_attention_state_machine_.getState() == StateMachine::State::STOP; if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{ - is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, - occlusion_stop_line_idx}; + return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } else { - return IntersectionModule::PeekingTowardOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, collision_stop_line_idx, - occlusion_stop_line_idx}; + return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stop_line_idx, + first_attention_stop_line_idx, + occlusion_stop_line_idx}; } } else { - if (is_stopped && approached_stop_line) { + if (is_stopped_at_default && approached_default_stop_line) { // start waiting at the first stop line before_creep_state_machine_.setState(StateMachine::State::GO); } + const auto occlusion_stop_line = planner_param_.occlusion.temporal_stop_before_attention_area + ? first_attention_stop_line_idx + : occlusion_stop_line_idx; return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, - occlusion_stop_line_idx}; + is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; } } else if (has_collision_with_margin) { return IntersectionModule::NonOccludedCollisionStop{ @@ -1054,7 +1098,7 @@ bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on) + const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; using lanelet::utils::getPolygonFromArcLength; @@ -1077,12 +1121,21 @@ bool IntersectionModule::checkCollision( const auto ego_poly = ego_lane.polygon2d().basicPolygon(); // check collision between predicted_path and ego_area - const double collision_start_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_start_margin_time - : planner_param_.collision_detection.normal.collision_start_margin_time; - const double collision_end_margin_time = - tl_arrow_solid_on ? planner_param_.collision_detection.relaxed.collision_end_margin_time - : planner_param_.collision_detection.normal.collision_end_margin_time; + const auto [collision_start_margin_time, collision_end_margin_time] = [&]() { + if (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.fully_prioritized.collision_start_margin_time, + planner_param_.collision_detection.fully_prioritized.collision_end_margin_time); + } + if (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) { + return std::make_pair( + planner_param_.collision_detection.partially_prioritized.collision_start_margin_time, + planner_param_.collision_detection.partially_prioritized.collision_end_margin_time); + } + return std::make_pair( + planner_param_.collision_detection.not_prioritized.collision_start_margin_time, + planner_param_.collision_detection.not_prioritized.collision_end_margin_time); + }(); bool collision_detected = false; for (const auto & object : target_objects.objects) { for (const auto & predicted_path : object.kinematics.predicted_paths) { @@ -1182,7 +1235,8 @@ bool IntersectionModule::isOcclusionCleared( const lanelet::CompoundPolygon3d & first_attention_area, const util::InterpolatedPathInfo & interpolated_path_info, const std::vector & lane_divisions, - const std::vector & parked_attention_objects, + [[maybe_unused]] const std::vector & + blocking_attention_objects, const double occlusion_dist_thr) { const auto & path_ip = interpolated_path_info.path; @@ -1200,19 +1254,13 @@ bool IntersectionModule::isOcclusionCleared( first_inside_attention_idx_ip_opt ? std::make_pair(first_inside_attention_idx_ip_opt.value(), std::get<1>(lane_interval_ip)) : lane_interval_ip; + const auto [lane_start_idx, lane_end_idx] = lane_attention_interval_ip; const int width = occ_grid.info.width; const int height = occ_grid.info.height; const double resolution = occ_grid.info.resolution; const auto & origin = occ_grid.info.origin.position; - // NOTE: interesting area is set to 0 for later masking - cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); - cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); - - // (1) prepare detection area mask - // attention: 255 - // non-attention: 0 Polygon2d grid_poly; grid_poly.outer().emplace_back(origin.x, origin.y); grid_poly.outer().emplace_back(origin.x + (width - 1) * resolution, origin.y); @@ -1222,10 +1270,9 @@ bool IntersectionModule::isOcclusionCleared( grid_poly.outer().emplace_back(origin.x, origin.y); bg::correct(grid_poly); - std::vector> attention_area_cv_polygons; - for (const auto & attention_area : attention_areas) { - const auto area2d = lanelet::utils::to2D(attention_area); - Polygon2d area2d_poly; + auto findCommonCvPolygons = + [&](const auto & area2d, std::vector> & cv_polygons) -> void { + tier4_autoware_utils::Polygon2d area2d_poly; for (const auto & p : area2d) { area2d_poly.outer().emplace_back(p.x(), p.y()); } @@ -1234,21 +1281,32 @@ bool IntersectionModule::isOcclusionCleared( std::vector common_areas; bg::intersection(area2d_poly, grid_poly, common_areas); if (common_areas.empty()) { - continue; + return; } for (size_t i = 0; i < common_areas.size(); ++i) { common_areas[i].outer().push_back(common_areas[i].outer().front()); bg::correct(common_areas[i]); } for (const auto & common_area : common_areas) { - std::vector attention_area_cv_polygon; + std::vector cv_polygon; for (const auto & p : common_area.outer()) { const int idx_x = static_cast((p.x() - origin.x) / resolution); const int idx_y = static_cast((p.y() - origin.y) / resolution); - attention_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); + cv_polygon.emplace_back(idx_x, height - 1 - idx_y); } - attention_area_cv_polygons.push_back(attention_area_cv_polygon); + cv_polygons.push_back(cv_polygon); } + }; + + // (1) prepare detection area mask + // attention: 255 + // non-attention: 0 + // NOTE: interesting area is set to 255 for later masking + cv::Mat attention_mask(width, height, CV_8UC1, cv::Scalar(0)); + std::vector> attention_area_cv_polygons; + for (const auto & attention_area : attention_areas) { + const auto area2d = lanelet::utils::to2D(attention_area); + findCommonCvPolygons(area2d, attention_area_cv_polygons); } for (const auto & poly : attention_area_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(255), cv::LINE_AA); @@ -1258,30 +1316,7 @@ bool IntersectionModule::isOcclusionCleared( std::vector> adjacent_lane_cv_polygons; for (const auto & adjacent_lanelet : adjacent_lanelets) { const auto area2d = adjacent_lanelet.polygon2d().basicPolygon(); - Polygon2d area2d_poly; - for (const auto & p : area2d) { - area2d_poly.outer().emplace_back(p.x(), p.y()); - } - area2d_poly.outer().push_back(area2d_poly.outer().front()); - bg::correct(area2d_poly); - std::vector common_areas; - bg::intersection(area2d_poly, grid_poly, common_areas); - if (common_areas.empty()) { - continue; - } - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - for (const auto & common_area : common_areas) { - std::vector adjacent_lane_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = std::floor((p.x() - origin.x) / resolution); - const int idx_y = std::floor((p.y() - origin.y) / resolution); - adjacent_lane_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - adjacent_lane_cv_polygons.push_back(adjacent_lane_cv_polygon); - } + findCommonCvPolygons(area2d, adjacent_lane_cv_polygons); } for (const auto & poly : adjacent_lane_cv_polygons) { cv::fillPoly(attention_mask, poly, cv::Scalar(0), cv::LINE_AA); @@ -1289,6 +1324,10 @@ bool IntersectionModule::isOcclusionCleared( // (2) prepare unknown mask // In OpenCV the pixel at (X=x, Y=y) (with left-upper origin) is accessed by img[y, x] + // unknown: 255 + // not-unknown: 0 + cv::Mat unknown_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); + cv::Mat unknown_mask(width, height, CV_8UC1, cv::Scalar(0)); for (int x = 0; x < width; x++) { for (int y = 0; y < height; y++) { const int idx = y * width + x; @@ -1296,159 +1335,21 @@ bool IntersectionModule::isOcclusionCleared( if ( planner_param_.occlusion.free_space_max <= intensity && intensity < planner_param_.occlusion.occupied_min) { - unknown_mask.at(height - 1 - y, x) = 255; + unknown_mask_raw.at(height - 1 - y, x) = 255; } } } - - // (3) occlusion mask - cv::Mat occlusion_mask_raw(width, height, CV_8UC1, cv::Scalar(0)); - cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask_raw); - // (3.1) apply morphologyEx - cv::Mat occlusion_mask; + // (2.1) apply morphologyEx const int morph_size = static_cast(planner_param_.occlusion.denoise_kernel / resolution); cv::morphologyEx( - occlusion_mask_raw, occlusion_mask, cv::MORPH_OPEN, + unknown_mask_raw, unknown_mask, cv::MORPH_OPEN, cv::getStructuringElement(cv::MORPH_RECT, cv::Size(morph_size, morph_size))); - // (4) create distance grid - // value: 0 - 254: signed distance representing [distance_min, distance_max] - // 255: undefined value - const double distance_max = std::hypot(width * resolution / 2, height * resolution / 2); - const double distance_min = -distance_max; - const int undef_pixel = 255; - const int max_cost_pixel = 254; - auto dist2pixel = [=](const double dist) { - return std::min( - max_cost_pixel, - static_cast((dist - distance_min) / (distance_max - distance_min) * max_cost_pixel)); - }; - auto pixel2dist = [=](const int pixel) { - return pixel * 1.0 / max_cost_pixel * (distance_max - distance_min) + distance_min; - }; - const int zero_dist_pixel = dist2pixel(0.0); - const int parked_vehicle_pixel = zero_dist_pixel - 1; // magic - - auto coord2index = [&](const double x, const double y) { - const int idx_x = (x - origin.x) / resolution; - const int idx_y = (y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); - if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); - return std::make_tuple(true, idx_x, idx_y); - }; - - cv::Mat distance_grid(width, height, CV_8UC1, cv::Scalar(undef_pixel)); - cv::Mat projection_ind_grid(width, height, CV_32S, cv::Scalar(-1)); - - // (4.1) fill zero_dist_pixel on the path - const auto [lane_start, lane_end] = lane_attention_interval_ip; - for (int i = static_cast(lane_end); i >= static_cast(lane_start); i--) { - const auto & path_pos = path_ip.points.at(i).point.pose.position; - const int idx_x = (path_pos.x - origin.x) / resolution; - const int idx_y = (path_pos.y - origin.y) / resolution; - if (idx_x < 0 || idx_x >= width) continue; - if (idx_y < 0 || idx_y >= height) continue; - distance_grid.at(height - 1 - idx_y, idx_x) = zero_dist_pixel; - projection_ind_grid.at(height - 1 - idx_y, idx_x) = i; - } - - // (4.2) fill parked_vehicle_pixel to parked_vehicles (both positive and negative) - for (const auto & parked_attention_object : parked_attention_objects) { - const auto obj_poly = tier4_autoware_utils::toPolygon2d(parked_attention_object); - std::vector common_areas; - bg::intersection(obj_poly, grid_poly, common_areas); - if (common_areas.empty()) continue; - for (size_t i = 0; i < common_areas.size(); ++i) { - common_areas[i].outer().push_back(common_areas[i].outer().front()); - bg::correct(common_areas[i]); - } - std::vector> parked_attention_object_area_cv_polygons; - for (const auto & common_area : common_areas) { - std::vector parked_attention_object_area_cv_polygon; - for (const auto & p : common_area.outer()) { - const int idx_x = static_cast((p.x() - origin.x) / resolution); - const int idx_y = static_cast((p.y() - origin.y) / resolution); - parked_attention_object_area_cv_polygon.emplace_back(idx_x, height - 1 - idx_y); - } - parked_attention_object_area_cv_polygons.push_back(parked_attention_object_area_cv_polygon); - } - for (const auto & poly : parked_attention_object_area_cv_polygons) { - cv::fillPoly(distance_grid, poly, cv::Scalar(parked_vehicle_pixel), cv::LINE_AA); - } - } - - for (const auto & lane_division : lane_divisions) { - const auto & divisions = lane_division.divisions; - for (const auto & division : divisions) { - bool is_in_grid = false; - bool zero_dist_cell_found = false; - int projection_ind = -1; - std::optional> cost_prev_checkpoint = - std::nullopt; // cost, x, y, projection_ind - for (auto point = division.begin(); point != division.end(); point++) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - // exited grid just now - if (is_in_grid && !valid) break; - - // still not entering grid - if (!is_in_grid && !valid) continue; - - // From here, "valid" - const int pixel = distance_grid.at(height - 1 - idx_y, idx_x); - - // entered grid for 1st time - if (!is_in_grid) { - assert(pixel == undef_pixel || pixel == zero_dist_pixel); - is_in_grid = true; - if (pixel == undef_pixel) { - continue; - } - } - - if (pixel == zero_dist_pixel) { - zero_dist_cell_found = true; - projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - assert(projection_ind >= 0); - cost_prev_checkpoint = - std::make_optional>(0.0, x, y, projection_ind); - continue; - } - - // hit positive parked vehicle - if (zero_dist_cell_found && pixel == parked_vehicle_pixel) { - while (point != division.end()) { - const double x = point->x(), y = point->y(); - const auto [valid, idx_x, idx_y] = coord2index(x, y); - if (valid) occlusion_mask.at(height - 1 - idx_y, idx_x) = 0; - point++; - } - break; - } - - if (zero_dist_cell_found) { - // finally traversed to defined cell (first half) - const auto [prev_cost, prev_checkpoint_x, prev_checkpoint_y, prev_projection_ind] = - cost_prev_checkpoint.value(); - const double dy = y - prev_checkpoint_y, dx = x - prev_checkpoint_x; - double new_dist = prev_cost + std::hypot(dy, dx); - const int new_projection_ind = projection_ind_grid.at(height - 1 - idx_y, idx_x); - const double cur_dist = pixel2dist(pixel); - if (planner_param_.occlusion.do_dp && cur_dist < new_dist) { - new_dist = cur_dist; - if (new_projection_ind > 0) { - projection_ind = std::min(prev_projection_ind, new_projection_ind); - } - } - projection_ind_grid.at(height - 1 - idx_y, idx_x) = projection_ind; - distance_grid.at(height - 1 - idx_y, idx_x) = dist2pixel(new_dist); - cost_prev_checkpoint = std::make_optional>( - new_dist, x, y, projection_ind); - } - } - } - } + // (3) occlusion mask + cv::Mat occlusion_mask(width, height, CV_8UC1, cv::Scalar(0)); + cv::bitwise_and(attention_mask, unknown_mask, occlusion_mask); + // (4) extract occlusion polygons const auto & possible_object_bbox = planner_param_.occlusion.possible_object_bbox; const double possible_object_bbox_x = possible_object_bbox.at(0) / resolution; const double possible_object_bbox_y = possible_object_bbox.at(1) / resolution; @@ -1457,19 +1358,12 @@ bool IntersectionModule::isOcclusionCleared( cv::findContours(occlusion_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); std::vector> valid_contours; for (const auto & contour : contours) { - std::vector valid_contour; - for (const auto & p : contour) { - if (distance_grid.at(p.y, p.x) == undef_pixel) { - continue; - } - valid_contour.push_back(p); - } - if (valid_contour.size() <= 2) { + if (contour.size() <= 2) { continue; } std::vector approx_contour; cv::approxPolyDP( - valid_contour, approx_contour, + contour, approx_contour, std::round(std::min(possible_object_bbox_x, possible_object_bbox_y) / std::sqrt(2.0)), true); if (approx_contour.size() <= 2) continue; // check area @@ -1496,32 +1390,126 @@ bool IntersectionModule::isOcclusionCleared( } debug_data_.occlusion_polygons.push_back(polygon_msg); } + // (4.1) re-draw occluded cells using valid_contours + occlusion_mask = cv::Mat(width, height, CV_8UC1, cv::Scalar(0)); + for (unsigned i = 0; i < valid_contours.size(); ++i) { + // NOTE: drawContour does not work well + cv::fillPoly(occlusion_mask, valid_contours[i], cv::Scalar(255), cv::LINE_AA); + } - const int min_cost_thr = dist2pixel(occlusion_dist_thr); - int min_cost = undef_pixel - 1; - geometry_msgs::msg::Point nearest_occlusion_point; - for (const auto & occlusion_contour : valid_contours) { - for (const auto & p : occlusion_contour) { - const int pixel = static_cast(distance_grid.at(p.y, p.x)); - const bool occluded = (occlusion_mask.at(p.y, p.x) == 255); - if (pixel == undef_pixel || !occluded) { + auto coord2index = [&](const double x, const double y) { + const int idx_x = (x - origin.x) / resolution; + const int idx_y = (y - origin.y) / resolution; + if (idx_x < 0 || idx_x >= width) return std::make_tuple(false, -1, -1); + if (idx_y < 0 || idx_y >= height) return std::make_tuple(false, -1, -1); + return std::make_tuple(true, idx_x, idx_y); + }; + + // (5) find distance + // (5.1) discretize path_ip with resolution for computational cost + LineString2d path_linestring; + path_linestring.emplace_back( + path_ip.points.at(lane_start_idx).point.pose.position.x, + path_ip.points.at(lane_start_idx).point.pose.position.y); + { + auto prev_path_linestring_point = path_ip.points.at(lane_start_idx).point.pose.position; + for (auto i = lane_start_idx + 1; i <= lane_end_idx; i++) { + const auto path_linestring_point = path_ip.points.at(i).point.pose.position; + if ( + tier4_autoware_utils::calcDistance2d(prev_path_linestring_point, path_linestring_point) < + 1.0 /* rough tick for computational cost */) { continue; } - if (pixel < min_cost) { - min_cost = pixel; - nearest_occlusion_point.x = origin.x + p.x * resolution; - nearest_occlusion_point.y = origin.y + (height - 1 - p.y) * resolution; - nearest_occlusion_point.z = origin.z; + path_linestring.emplace_back(path_linestring_point.x, path_linestring_point.y); + prev_path_linestring_point = path_linestring_point; + } + } + + auto findNearestPointToProjection = + [](lanelet::ConstLineString2d division, const Point2d & projection, const double dist_thresh) { + double min_dist = std::numeric_limits::infinity(); + auto nearest = division.end(); + for (auto it = division.begin(); it != division.end(); it++) { + const double dist = std::hypot(it->x() - projection.x(), it->y() - projection.y()); + if (dist < min_dist) { + min_dist = dist; + nearest = it; + } + if (dist < dist_thresh) { + break; + } + } + return nearest; + }; + struct NearestOcclusionPoint + { + int lane_id; + int64 division_index; + double dist; + geometry_msgs::msg::Point point; + geometry_msgs::msg::Point projection; + } nearest_occlusion_point; + double min_dist = std::numeric_limits::infinity(); + for (const auto & lane_division : lane_divisions) { + const auto & divisions = lane_division.divisions; + const auto lane_id = lane_division.lane_id; + for (unsigned division_index = 0; division_index < divisions.size(); ++division_index) { + const auto & division = divisions.at(division_index); + LineString2d division_linestring; + auto division_point_it = division.begin(); + division_linestring.emplace_back(division_point_it->x(), division_point_it->y()); + for (auto point_it = division.begin(); point_it != division.end(); point_it++) { + if ( + std::hypot( + point_it->x() - division_point_it->x(), point_it->y() - division_point_it->y()) < + 3.0 /* rough tick for computational cost */) { + continue; + } + division_linestring.emplace_back(point_it->x(), point_it->y()); + division_point_it = point_it; + } + + // find the intersection point of lane_line and path + std::vector intersection_points; + boost::geometry::intersection(division_linestring, path_linestring, intersection_points); + if (intersection_points.empty()) { + continue; + } + const auto & projection_point = intersection_points.at(0); + const auto projection_it = + findNearestPointToProjection(division, projection_point, resolution); + if (projection_it == division.end()) { + continue; + } + double acc_dist = 0.0; + auto acc_dist_it = projection_it; + for (auto point_it = projection_it; point_it != division.end(); point_it++) { + const double dist = + std::hypot(point_it->x() - acc_dist_it->x(), point_it->y() - acc_dist_it->y()); + acc_dist += dist; + acc_dist_it = point_it; + const auto [valid, idx_x, idx_y] = coord2index(point_it->x(), point_it->y()); + // TODO(Mamoru Sobue): add handling for blocking vehicles + if (!valid) continue; + if (occlusion_mask.at(height - 1 - idx_y, idx_x) == 255) { + if (acc_dist < min_dist) { + min_dist = acc_dist; + nearest_occlusion_point = { + lane_id, std::distance(division.begin(), point_it), acc_dist, + tier4_autoware_utils::createPoint(point_it->x(), point_it->y(), origin.z), + tier4_autoware_utils::createPoint(projection_it->x(), projection_it->y(), origin.z)}; + } + } } } } - if (min_cost > min_cost_thr) { + if (min_dist == std::numeric_limits::infinity() || min_dist > occlusion_dist_thr) { return true; - } else { - debug_data_.nearest_occlusion_point = nearest_occlusion_point; - return false; } + debug_data_.nearest_occlusion_projection = + std::make_pair(nearest_occlusion_point.point, nearest_occlusion_point.projection); + return false; } /* diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 8063300b9b..89444bb71c 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -79,16 +79,21 @@ class IntersectionModule : public SceneModuleInterface //! minimum confidence value of predicted path to use for collision detection double minimum_ego_predicted_velocity; //! used to calculate ego's future velocity profile double state_transit_margin_time; - struct Normal + struct FullyPrioritized { double collision_start_margin_time; //! start margin time to check collision double collision_end_margin_time; //! end margin time to check collision - } normal; - struct Relaxed + } fully_prioritized; + struct PartiallyPrioritized { - double collision_start_margin_time; - double collision_end_margin_time; - } relaxed; + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } partially_prioritized; + struct NotPrioritized + { + double collision_start_margin_time; //! start margin time to check collision + double collision_end_margin_time; //! end margin time to check collision + } not_prioritized; double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr } collision_detection; struct Occlusion @@ -108,6 +113,7 @@ class IntersectionModule : public SceneModuleInterface std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; double stop_release_margin_time; + bool temporal_stop_before_attention_area; } occlusion; }; @@ -136,15 +142,19 @@ class IntersectionModule : public SceneModuleInterface // NOTE: if intersection_occlusion is disapproved externally through RTC, // it indicates "is_forcefully_occluded" bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; struct OccludedCollisionStop { bool is_actually_occlusion_cleared{false}; + bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; size_t collision_stop_line_idx{0}; + size_t first_attention_stop_line_idx{0}; size_t occlusion_stop_line_idx{0}; }; struct Safe @@ -217,6 +227,7 @@ class IntersectionModule : public SceneModuleInterface StateMachine collision_state_machine_; //! for stable collision checking StateMachine before_creep_state_machine_; //! for two phase stop StateMachine occlusion_stop_state_machine_; + StateMachine temporal_stop_before_attention_state_machine_; // NOTE: uuid_ is base member // for stuck vehicle detection @@ -250,7 +261,7 @@ class IntersectionModule : public SceneModuleInterface const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const autoware_auto_perception_msgs::msg::PredictedObjects & target_objects, const util::PathLanelets & path_lanelets, const int closest_idx, const double time_delay, - const bool tl_arrow_solid_on); + const util::TrafficPrioritizedLevel & traffic_prioritized_level); bool isOcclusionCleared( const nav_msgs::msg::OccupancyGrid & occ_grid, diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index 10ad395953..bb54d829cc 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -134,12 +135,11 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position); - constexpr double distance_threshold = 2.0; if ( - signed_arc_dist_to_stop_point < distance_threshold && + signed_arc_dist_to_stop_point < planner_param_.stop_distance_threshold && planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) { state_machine_.setState(StateMachine::State::GO); - if (signed_arc_dist_to_stop_point < -distance_threshold) { + if (signed_arc_dist_to_stop_point < -planner_param_.stop_distance_threshold) { RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO"); } } diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index e00b2a8b88..19e7b92010 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -58,6 +58,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface double attention_area_length; double stop_line_margin; double stop_duration_sec; + double stop_distance_threshold; double path_interpolation_ds; double occlusion_attention_area_length; bool consider_wrong_direction_vehicle; diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index ce2d7252ac..a3bebecbd2 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -23,9 +23,11 @@ #include #include #include +#include #include #include +#include #include #include @@ -40,7 +42,6 @@ #include #include #include - namespace behavior_velocity_planner { namespace bg = boost::geometry; @@ -278,6 +279,8 @@ std::optional generateIntersectionStopLines( } } } + const auto first_attention_stop_line_ip = static_cast(occlusion_peeking_line_ip_int); + const bool first_attention_stop_line_valid = occlusion_peeking_line_valid; occlusion_peeking_line_ip_int += std::ceil(peeking_offset / ds); const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); @@ -323,6 +326,7 @@ std::optional generateIntersectionStopLines( size_t closest_idx{0}; size_t stuck_stop_line{0}; size_t default_stop_line{0}; + size_t first_attention_stop_line{0}; size_t occlusion_peeking_stop_line{0}; size_t pass_judge_line{0}; }; @@ -332,6 +336,7 @@ std::optional generateIntersectionStopLines( {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, + {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line}, {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, }; @@ -366,6 +371,10 @@ std::optional generateIntersectionStopLines( if (default_stop_line_valid) { intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; } + if (first_attention_stop_line_valid) { + intersection_stop_lines.first_attention_stop_line = + intersection_stop_lines_temp.first_attention_stop_line; + } if (occlusion_peeking_line_valid) { intersection_stop_lines.occlusion_peeking_stop_line = intersection_stop_lines_temp.occlusion_peeking_stop_line; @@ -764,12 +773,11 @@ bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane) return tl_id.has_value(); } -bool isTrafficLightArrowActivated( +TrafficPrioritizedLevel getTrafficPrioritizedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos) { using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; - const auto & turn_direction = lane.attributeOr("turn_direction", "else"); std::optional tl_id = std::nullopt; for (auto && tl_reg_elem : lane.regulatoryElementsAs()) { tl_id = tl_reg_elem->id(); @@ -777,24 +785,28 @@ bool isTrafficLightArrowActivated( } if (!tl_id) { // this lane has no traffic light - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } const auto tl_info_it = tl_infos.find(tl_id.value()); if (tl_info_it == tl_infos.end()) { // the info of this traffic light is not available - return false; + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } const auto & tl_info = tl_info_it->second; + bool has_amber_signal{false}; for (auto && tl_light : tl_info.signal.elements) { - if (tl_light.color != TrafficSignalElement::GREEN) continue; - if (tl_light.status != TrafficSignalElement::SOLID_ON) continue; - if (turn_direction == std::string("left") && tl_light.shape == TrafficSignalElement::LEFT_ARROW) - return true; - if ( - turn_direction == std::string("right") && tl_light.shape == TrafficSignalElement::RIGHT_ARROW) - return true; + if (tl_light.color == TrafficSignalElement::AMBER) { + has_amber_signal = true; + } + if (tl_light.color == TrafficSignalElement::RED) { + // NOTE: Return here since the red signal has the highest priority. + return TrafficPrioritizedLevel::FULLY_PRIORITIZED; + } } - return false; + if (has_amber_signal) { + return TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED; + } + return TrafficPrioritizedLevel::NOT_PRIORITIZED; } std::vector generateDetectionLaneDivisions( @@ -1177,9 +1189,9 @@ double calcDistanceUntilIntersectionLanelet( } void IntersectionLanelets::update( - const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info) + const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info) { - tl_arrow_solid_on_ = tl_arrow_solid_on; + is_prioritized_ = is_prioritized; // find the first conflicting/detection area polygon intersecting the path const auto & path = interpolated_path_info.path; const auto & lane_interval = interpolated_path_info.lane_id_interval.value(); diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index e766da6651..72e96876f0 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -111,7 +111,8 @@ std::optional getIntersectionArea( lanelet::ConstLanelet assigned_lane, lanelet::LaneletMapConstPtr lanelet_map_ptr); bool hasAssociatedTrafficLight(lanelet::ConstLanelet lane); -bool isTrafficLightArrowActivated( + +TrafficPrioritizedLevel getTrafficPrioritizedLevel( lanelet::ConstLanelet lane, const std::map & tl_infos); std::vector generateDetectionLaneDivisions( diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 64be330a99..4135884f03 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -34,22 +34,22 @@ namespace behavior_velocity_planner::util struct DebugData { - std::optional collision_stop_wall_pose = std::nullopt; - std::optional occlusion_stop_wall_pose = std::nullopt; - std::optional occlusion_first_stop_wall_pose = std::nullopt; - std::optional pass_judge_wall_pose = std::nullopt; - std::optional> attention_area = std::nullopt; - std::optional intersection_area = std::nullopt; - std::optional ego_lane = std::nullopt; - std::optional> adjacent_area = std::nullopt; - std::optional stuck_vehicle_detect_area = std::nullopt; - std::optional candidate_collision_ego_lane_polygon = std::nullopt; + std::optional collision_stop_wall_pose{std::nullopt}; + std::optional occlusion_stop_wall_pose{std::nullopt}; + std::optional occlusion_first_stop_wall_pose{std::nullopt}; + std::optional pass_judge_wall_pose{std::nullopt}; + std::optional> attention_area{std::nullopt}; + std::optional intersection_area{std::nullopt}; + std::optional ego_lane{std::nullopt}; + std::optional> adjacent_area{std::nullopt}; + std::optional stuck_vehicle_detect_area{std::nullopt}; + std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - std::optional nearest_occlusion_point = std::nullopt; - std::optional nearest_occlusion_projection_point = std::nullopt; std::vector occlusion_polygons; + std::optional> + nearest_occlusion_projection{std::nullopt}; }; struct InterpolatedPathInfo @@ -64,20 +64,20 @@ struct InterpolatedPathInfo struct IntersectionLanelets { public: - void update(const bool tl_arrow_solid_on, const InterpolatedPathInfo & interpolated_path_info); + void update(const bool is_prioritized, const InterpolatedPathInfo & interpolated_path_info); const lanelet::ConstLanelets & attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : attention_; + return is_prioritized_ ? attention_non_preceding_ : attention_; } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } const lanelet::ConstLanelets & occlusion_attention() const { - return tl_arrow_solid_on_ ? attention_non_preceding_ : occlusion_attention_; + return is_prioritized_ ? attention_non_preceding_ : occlusion_attention_; } const std::vector & attention_area() const { - return tl_arrow_solid_on_ ? attention_non_preceding_area_ : attention_area_; + return is_prioritized_ ? attention_non_preceding_area_ : attention_area_; } const std::vector & conflicting_area() const { @@ -110,7 +110,7 @@ struct IntersectionLanelets // the first area intersecting with the path // even if lane change/re-routing happened on the intersection, these areas area are supposed to // be invariant under the 'associative' lanes. - bool tl_arrow_solid_on_ = false; + bool is_prioritized_ = false; std::optional first_conflicting_area_ = std::nullopt; std::optional first_attention_area_ = std::nullopt; }; @@ -128,8 +128,10 @@ struct IntersectionStopLines size_t closest_idx{0}; // NOTE: null if path does not conflict with first_conflicting_area std::optional stuck_stop_line{std::nullopt}; - // NOTE: null if path is over map stop_line OR its value is calculated negative area + // NOTE: null if path is over map stop_line OR its value is calculated negative std::optional default_stop_line{std::nullopt}; + // NOTE: null if the index is calculated negative + std::optional first_attention_stop_line{std::nullopt}; // NOTE: null if footprints do not change from outside to inside of detection area std::optional occlusion_peeking_stop_line{std::nullopt}; // if the value is calculated negative, its value is 0 @@ -151,6 +153,14 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the path }; +enum class TrafficPrioritizedLevel { + // The target lane's traffic signal is red or the ego's traffic signal has an arrow. + FULLY_PRIORITIZED = 0, + // The target lane's traffic signal is amber + PARTIALLY_PRIORITIZED, + // The target lane's traffic signal is green + NOT_PRIORITIZED +}; } // namespace behavior_velocity_planner::util #endif // UTIL_TYPE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 2c57ff9458..f1fa4c47e9 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "manager.hpp" +#include #include #include diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp index 8109b99166..0d0e7cc6ca 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -18,6 +18,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" #include "util.hpp" +#include #include namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 889d0dbb06..4c5efad94d 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -18,6 +18,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include +#include + +#include #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index e8bf83868d..9e51afca2d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -17,8 +17,8 @@ #include #include #include +#include #include - #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index 60d659497e..d5e9eeddb6 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "manager.hpp" +#include #include #include diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index 3e24689f2e..ceef50094e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -18,11 +18,12 @@ #include #include #include +#include #include #include +#include #include - #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index 527f6f0214..a6e13d0dfd 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -32,6 +32,7 @@ using builtin_interfaces::msg::Time; using BasicPolygons = std::vector; using occlusion_spot_utils::PossibleCollisionInfo; using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerOrientation; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp index 029b516e93..fa8be96c02 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 2ebfa4ecbe..62114f5a95 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -16,16 +16,20 @@ #include #include -#include #include #include +#include + +#include +#include +#include + #include #include #include #include #include - namespace behavior_velocity_planner::out_of_lane { double distance_along_path(const EgoData & ego_data, const size_t target_idx) diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 1d1fe8bea9..2898595c2e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -25,6 +25,7 @@ #include #include +#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 48869e5e39..13bad0d047 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -17,6 +17,8 @@ #include "types.hpp" +#include + #include namespace behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 9924319b14..7f3b69f408 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -17,6 +17,7 @@ #include #include +#include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 280a832f6d..013643b6bd 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index fbdb17f93d..ee7dd7c07a 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -2,25 +2,43 @@ cmake_minimum_required(VERSION 3.14) project(behavior_velocity_planner) find_package(autoware_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces( + ${PROJECT_NAME} + "srv/LoadPlugin.srv" + "srv/UnloadPlugin.srv" + DEPENDENCIES +) + autoware_package() -ament_auto_add_library(${PROJECT_NAME} SHARED +ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/node.cpp src/planner_manager.cpp ) -rclcpp_components_register_node(${PROJECT_NAME} +rclcpp_components_register_node(${PROJECT_NAME}_lib PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE ${PROJECT_NAME}_node ) +if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) + rosidl_target_interfaces(${PROJECT_NAME}_lib + ${PROJECT_NAME} "rosidl_typesupport_cpp") +else() + rosidl_get_typesupport_target( + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_lib "${cpp_typesupport_target}") +endif() + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/src/test_node_interface.cpp ) target_link_libraries(test_${PROJECT_NAME} gtest_main - ${PROJECT_NAME} + ${PROJECT_NAME}_lib ) target_include_directories(test_${PROJECT_NAME} PRIVATE src) endif() diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index e8377f841a..276e8f8e31 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -22,5 +22,6 @@ # behavior_velocity_planner::OcclusionSpotModulePlugin # behavior_velocity_planner::RunOutModulePlugin # behavior_velocity_planner::SpeedBumpModulePlugin + # behavior_velocity_planner::TemplateModulePlugin - behavior_velocity_planner::OutOfLaneModulePlugin - behavior_velocity_planner::NoDrivableLaneModulePlugin diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index b517b77b32..69d9c27331 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -19,6 +19,7 @@ + @@ -40,6 +41,7 @@ + @@ -68,6 +70,7 @@ + diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 5a307398e9..d83f74a71b 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -33,6 +33,8 @@ autoware_cmake eigen3_cmake_module + rosidl_default_generators + autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs @@ -60,6 +62,8 @@ tier4_v2x_msgs visualization_msgs + rosidl_default_runtime + ament_cmake_ros ament_lint_auto autoware_lint_common @@ -77,6 +81,9 @@ behavior_velocity_traffic_light_module behavior_velocity_virtual_traffic_light_module behavior_velocity_walkway_module + + + rosidl_interface_packages ament_cmake diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index c4f4edd264..f30043ad42 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -27,7 +28,6 @@ #include #include #include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -75,6 +75,8 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio planner_data_(*this) { using std::placeholders::_1; + using std::placeholders::_2; + // Trigger Subscriber trigger_sub_path_with_lane_id_ = this->create_subscription( @@ -118,6 +120,12 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio "~/input/occupancy_grid", 1, std::bind(&BehaviorVelocityPlannerNode::onOccupancyGrid, this, _1), createSubscriptionOptions(this)); + srv_load_plugin_ = create_service( + "~/service/load_plugin", std::bind(&BehaviorVelocityPlannerNode::onLoadPlugin, this, _1, _2)); + srv_unload_plugin_ = create_service( + "~/service/unload_plugin", + std::bind(&BehaviorVelocityPlannerNode::onUnloadPlugin, this, _1, _2)); + // set velocity smoother param onParam(); @@ -145,6 +153,22 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio } } +void BehaviorVelocityPlannerNode::onLoadPlugin( + const LoadPlugin::Request::SharedPtr request, + [[maybe_unused]] const LoadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.launchScenePlugin(*this, request->plugin_name); +} + +void BehaviorVelocityPlannerNode::onUnloadPlugin( + const UnloadPlugin::Request::SharedPtr request, + [[maybe_unused]] const UnloadPlugin::Response::SharedPtr response) +{ + std::unique_lock lk(mutex_); + planner_manager_.removeScenePlugin(*this, request->plugin_name); +} + // NOTE: argument planner_data must not be referenced for multithreading bool BehaviorVelocityPlannerNode::isDataReady( const PlannerData planner_data, rclcpp::Clock clock) const diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index bf7c53f974..bdc280434e 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -17,6 +17,8 @@ #include "planner_manager.hpp" +#include +#include #include #include @@ -38,11 +40,15 @@ #include #include #include +#include namespace behavior_velocity_planner { using autoware_auto_mapping_msgs::msg::HADMapBin; +using behavior_velocity_planner::srv::LoadPlugin; +using behavior_velocity_planner::srv::UnloadPlugin; using tier4_planning_msgs::msg::VelocityLimit; + class BehaviorVelocityPlannerNode : public rclcpp::Node { public: @@ -103,6 +109,14 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node HADMapBin::ConstSharedPtr map_ptr_{nullptr}; bool has_received_map_; + rclcpp::Service::SharedPtr srv_load_plugin_; + rclcpp::Service::SharedPtr srv_unload_plugin_; + void onUnloadPlugin( + const UnloadPlugin::Request::SharedPtr request, + const UnloadPlugin::Response::SharedPtr response); + void onLoadPlugin( + const LoadPlugin::Request::SharedPtr request, const LoadPlugin::Response::SharedPtr response); + // mutex for planner_data_ std::mutex mutex_; diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index 39d4dc345e..e1c801a27d 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -60,12 +60,42 @@ void BehaviorVelocityPlannerManager::launchScenePlugin( if (plugin_loader_.isClassAvailable(name)) { const auto plugin = plugin_loader_.createSharedInstance(name); plugin->init(node); + + // Check if the plugin is already registered. + for (const auto & running_plugin : scene_manager_plugins_) { + if (plugin->getModuleName() == running_plugin->getModuleName()) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + } + + // register scene_manager_plugins_.push_back(plugin); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); } else { RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); } } +void BehaviorVelocityPlannerManager::removeScenePlugin( + rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if( + scene_manager_plugins_.begin(), scene_manager_plugins_.end(), + [&](const std::shared_ptr plugin) { + return plugin->getModuleName() == name; + }); + + if (it == scene_manager_plugins_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + scene_manager_plugins_.erase(it, scene_manager_plugins_.end()); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager::planPathVelocity( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path_msg) diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index e57d90d5cd..7485e5faba 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -43,6 +43,7 @@ class BehaviorVelocityPlannerManager public: BehaviorVelocityPlannerManager(); void launchScenePlugin(rclcpp::Node & node, const std::string & name); + void removeScenePlugin(rclcpp::Node & node, const std::string & name); autoware_auto_planning_msgs::msg::PathWithLaneId planPathVelocity( const std::shared_ptr & planner_data, diff --git a/planning/behavior_velocity_planner/srv/LoadPlugin.srv b/planning/behavior_velocity_planner/srv/LoadPlugin.srv new file mode 100644 index 0000000000..7b9f08ab0d --- /dev/null +++ b/planning/behavior_velocity_planner/srv/LoadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/behavior_velocity_planner/srv/UnloadPlugin.srv b/planning/behavior_velocity_planner/srv/UnloadPlugin.srv new file mode 100644 index 0000000000..7b9f08ab0d --- /dev/null +++ b/planning/behavior_velocity_planner/srv/UnloadPlugin.srv @@ -0,0 +1,3 @@ +string plugin_name +--- +bool success diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/behavior_velocity_planner_common/CMakeLists.txt index 334ee35329..e958df74af 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/behavior_velocity_planner_common/CMakeLists.txt @@ -5,7 +5,12 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED + src/scene_module_interface.cpp + src/velocity_factor_interface.cpp src/utilization/path_utilization.cpp + src/utilization/trajectory_utils.cpp + src/utilization/arc_lane_util.cpp + src/utilization/boost_geometry_helper.cpp src/utilization/util.cpp src/utilization/debug.cpp ) diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 2668d83b0f..620fc61ec5 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -18,7 +18,6 @@ #include "route_handler/route_handler.hpp" #include -#include #include #include @@ -38,13 +37,8 @@ #include -#include -#include -#include -#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 4735bb34c7..90e78215ca 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -16,14 +16,11 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ #include -#include #include #include #include #include #include -#include -#include #include #include @@ -35,10 +32,7 @@ #include #include -#include -#include #include -#include #include #include #include @@ -48,7 +42,6 @@ #include #include - namespace behavior_velocity_planner { @@ -56,7 +49,6 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using builtin_interfaces::msg::Time; using rtc_interface::RTCInterface; using tier4_autoware_utils::DebugPublisher; -using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; @@ -67,15 +59,7 @@ class SceneModuleInterface { public: explicit SceneModuleInterface( - const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - : module_id_(module_id), - activated_(false), - safe_(false), - distance_(std::numeric_limits::lowest()), - logger_(logger), - clock_(clock) - { - } + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); virtual ~SceneModuleInterface() = default; virtual bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) = 0; @@ -125,44 +109,14 @@ class SceneModuleInterface void setSafe(const bool safe) { safe_ = safe; } void setDistance(const double distance) { distance_ = distance; } - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold); - } + size_t findEgoSegmentIndex( + const std::vector & points) const; }; class SceneModuleManagerInterface { public: - SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name) - : node_(node), clock_(node.get_clock()), logger_(node.get_logger()) - { - const auto ns = std::string("~/debug/") + module_name; - pub_debug_ = node.create_publisher(ns, 1); - if (!node.has_parameter("is_publish_debug_path")) { - is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); - } else { - is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); - } - if (is_publish_debug_path_) { - pub_debug_path_ = node.create_publisher( - std::string("~/debug/path_with_lane_id/") + module_name, 1); - } - pub_virtual_wall_ = node.create_publisher( - std::string("~/virtual_wall/") + module_name, 5); - pub_velocity_factor_ = node.create_publisher( - std::string("/planning/velocity_factors/") + module_name, 1); - pub_stop_reason_ = - node.create_publisher("~/output/stop_reasons", 1); - pub_infrastructure_commands_ = - node.create_publisher( - "~/output/infrastructure_commands", 1); - - processing_time_publisher_ = std::make_shared(&node, "~/debug"); - } + SceneModuleManagerInterface(rclcpp::Node & node, [[maybe_unused]] const char * module_name); virtual ~SceneModuleManagerInterface() = default; @@ -172,13 +126,7 @@ class SceneModuleManagerInterface void updateSceneModuleInstances( const std::shared_ptr & planner_data, - const autoware_auto_planning_msgs::msg::PathWithLaneId & path) - { - planner_data_ = planner_data; - - launchNewModules(path); - deleteExpiredModules(path); - } + const autoware_auto_planning_msgs::msg::PathWithLaneId & path); virtual void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) { @@ -186,120 +134,26 @@ class SceneModuleManagerInterface } protected: - virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path) - { - StopWatch stop_watch; - stop_watch.tic("Total"); - visualization_msgs::msg::MarkerArray debug_marker_array; - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; - autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; - stop_reason_array.header.frame_id = "map"; - stop_reason_array.header.stamp = clock_->now(); - velocity_factor_array.header.frame_id = "map"; - velocity_factor_array.header.stamp = clock_->now(); - - tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; - infrastructure_command_array.stamp = clock_->now(); - - first_stop_path_point_index_ = static_cast(path->points.size()) - 1; - for (const auto & scene_module : scene_modules_) { - tier4_planning_msgs::msg::StopReason stop_reason; - scene_module->resetVelocityFactor(); - scene_module->setPlannerData(planner_data_); - scene_module->modifyPathVelocity(path, &stop_reason); - - // The velocity factor must be called after modifyPathVelocity. - const auto velocity_factor = scene_module->getVelocityFactor(); - if (velocity_factor.type != VelocityFactor::UNKNOWN) { - velocity_factor_array.factors.emplace_back(velocity_factor); - } - if (stop_reason.reason != "") { - stop_reason_array.stop_reasons.emplace_back(stop_reason); - } - - if (const auto command = scene_module->getInfrastructureCommand()) { - infrastructure_command_array.commands.push_back(*command); - } - - if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { - first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex(); - } - - for (const auto & marker : scene_module->createDebugMarkerArray().markers) { - debug_marker_array.markers.push_back(marker); - } - - virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); - } - - if (!stop_reason_array.stop_reasons.empty()) { - pub_stop_reason_->publish(stop_reason_array); - } - pub_velocity_factor_->publish(velocity_factor_array); - pub_infrastructure_commands_->publish(infrastructure_command_array); - pub_debug_->publish(debug_marker_array); - if (is_publish_debug_path_) { - autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; - debug_path.header = path->header; - debug_path.points = path->points; - pub_debug_path_->publish(debug_path); - } - pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); - processing_time_publisher_->publish( - std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); - } + virtual void modifyPathVelocity(autoware_auto_planning_msgs::msg::PathWithLaneId * path); virtual void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; virtual std::function &)> getModuleExpiredFunction(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) - { - const auto isModuleExpired = getModuleExpiredFunction(path); - - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - unregisterModule(scene_module); - } - } - } + virtual void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path); bool isModuleRegistered(const int64_t module_id) { return registered_module_id_set_.count(module_id) != 0; } - void registerModule(const std::shared_ptr & scene_module) - { - RCLCPP_INFO( - logger_, "register task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.emplace(scene_module->getModuleId()); - scene_modules_.insert(scene_module); - } + void registerModule(const std::shared_ptr & scene_module); - void unregisterModule(const std::shared_ptr & scene_module) - { - RCLCPP_INFO( - logger_, "unregister task: module = %s, id = %lu", getModuleName(), - scene_module->getModuleId()); - registered_module_id_set_.erase(scene_module->getModuleId()); - scene_modules_.erase(scene_module); - } + void unregisterModule(const std::shared_ptr & scene_module); - template - size_t findEgoSegmentIndex(const std::vector & points) const - { - const auto & p = planner_data_; - return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - points, p->current_odometry->pose, p->ego_nearest_dist_threshold, - p->ego_nearest_yaw_threshold); - } + size_t findEgoSegmentIndex( + const std::vector & points) const; std::set> scene_modules_; std::set registered_module_id_set_; @@ -329,38 +183,17 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface { public: SceneModuleManagerInterfaceWithRTC( - rclcpp::Node & node, const char * module_name, const bool enable_rtc = true) - : SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) - { - } + rclcpp::Node & node, const char * module_name, const bool enable_rtc = true); - void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override - { - setActivation(); - modifyPathVelocity(path); - sendRTC(path->header.stamp); - } + void plan(autoware_auto_planning_msgs::msg::PathWithLaneId * path) override; protected: RTCInterface rtc_interface_; std::unordered_map map_uuid_; - virtual void sendRTC(const Time & stamp) - { - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); - } - publishRTCStatus(stamp); - } + virtual void sendRTC(const Time & stamp); - virtual void setActivation() - { - for (const auto & scene_module : scene_modules_) { - const UUID uuid = getUUID(scene_module->getModuleId()); - scene_module->setActivation(rtc_interface_.isActivated(uuid)); - } - } + virtual void setActivation(); void updateRTCStatus( const UUID & uuid, const bool safe, const double distance, const Time & stamp) @@ -372,45 +205,13 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); } - UUID getUUID(const int64_t & module_id) const - { - if (map_uuid_.count(module_id) == 0) { - const UUID uuid; - return uuid; - } - return map_uuid_.at(module_id); - } + UUID getUUID(const int64_t & module_id) const; - void generateUUID(const int64_t & module_id) - { - map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()}); - } + void generateUUID(const int64_t & module_id); - void removeUUID(const int64_t & module_id) - { - const auto result = map_uuid_.erase(module_id); - if (result == 0) { - RCLCPP_WARN_STREAM( - logger_, "[removeUUID] module_id = " << module_id << " is not registered."); - } - } + void removeUUID(const int64_t & module_id); - void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override - { - const auto isModuleExpired = getModuleExpiredFunction(path); - - // Copy container to avoid iterator corruption - // due to scene_modules_.erase() in unregisterModule() - const auto copied_scene_modules = scene_modules_; - - for (const auto & scene_module : copied_scene_modules) { - if (isModuleExpired(scene_module)) { - removeRTCStatus(getUUID(scene_module->getModuleId())); - removeUUID(scene_module->getModuleId()); - unregisterModule(scene_module); - } - } - } + void deleteExpiredModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index ffc216e863..63d7c08b94 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -16,22 +16,12 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ #include -#include #include #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#include -#else -#include - -#include -#endif - #include #include #include @@ -46,7 +36,7 @@ namespace bg = boost::geometry; namespace { -geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) +geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p) { geometry_msgs::msg::Point geom_p; geom_p.x = p.x(); @@ -55,73 +45,23 @@ geometry_msgs::msg::Point convertToGeomPoint(const Point2d & p) return geom_p; } -geometry_msgs::msg::Point operator+( - const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) -{ - geometry_msgs::msg::Point p; - p.x = p1.x + p2.x; - p.y = p1.y + p2.y; - p.z = p1.z + p2.z; - - return p; -} - -[[maybe_unused]] geometry_msgs::msg::Point operator*( - const geometry_msgs::msg::Point & p, const double v) -{ - geometry_msgs::msg::Point multiplied_p; - multiplied_p.x = p.x * v; - multiplied_p.y = p.y * v; - multiplied_p.z = p.z * v; - - return multiplied_p; -} - -[[maybe_unused]] geometry_msgs::msg::Point operator*( - const double v, const geometry_msgs::msg::Point & p) -{ - return p * v; -} } // namespace namespace arc_lane_utils { -using PathIndexWithPose = std::pair; // front index, pose -using PathIndexWithPoint2d = std::pair; // front index, point2d +using PathIndexWithPose = std::pair; // front index, pose +using PathIndexWithPoint2d = + std::pair; // front index, point2d using PathIndexWithPoint = std::pair; // front index, point2d using PathIndexWithOffset = std::pair; // front index, offset -inline double calcSignedDistance( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) -{ - Eigen::Affine3d map2p1; - tf2::fromMsg(p1, map2p1); - const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); - return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); -} +double calcSignedDistance( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2); // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) -inline boost::optional checkCollision( +boost::optional checkCollision( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, - const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) -{ - const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); - - if (det == 0.0) { - // collision is not one point. - return boost::none; - } - - const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; - const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; - - // check collision is outside the segment line - if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { - return boost::none; - } - - return p1 * (1.0 - t1) + p2 * t1; -} + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); template boost::optional findCollisionSegment( @@ -223,16 +163,9 @@ boost::optional findOffsetSegment( tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } -inline boost::optional findOffsetSegment( +boost::optional findOffsetSegment( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, - const double offset) -{ - if (offset >= 0) { - return findForwardOffsetSegment(path, index, offset); - } - - return findBackwardOffsetSegment(path, index, -offset); -} + const double offset); template geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffset & offset_segment) @@ -263,33 +196,10 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse return target_pose; } -inline boost::optional createTargetPoint( +boost::optional createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, - const size_t lane_id, const double margin, const double vehicle_offset) -{ - // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); - if (!collision_segment) { - // No collision - return {}; - } - - // Calculate offset length from stop line - // Use '-' to make the positive direction is forward - const double offset_length = -(margin + vehicle_offset); + const size_t lane_id, const double margin, const double vehicle_offset); - // Find offset segment - const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); - if (!offset_segment) { - // No enough path length - return {}; - } - - const auto target_pose = calcTargetPose(path, *offset_segment); - - const auto front_idx = offset_segment->first; - return std::make_pair(front_idx, target_pose); -} } // namespace arc_lane_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index 193890ec70..0f3cc7203e 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -24,24 +24,11 @@ #include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include #include -#include -#include - -#include #include - // cppcheck-suppress unknownMacro BOOST_GEOMETRY_REGISTER_POINT_3D(geometry_msgs::msg::Point, double, cs::cartesian, x, y, z) BOOST_GEOMETRY_REGISTER_POINT_3D( @@ -83,48 +70,13 @@ LineString2d to_bg2d(const std::vector & vec) return ps; } -inline Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) -{ - Polygon2d polygon; - - polygon.outer().push_back(left_line.front()); +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line); - for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { - polygon.outer().push_back(*itr); - } - - for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { - polygon.outer().push_back(*itr); - } +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale); - bg::correct(polygon); - return polygon; -} +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -inline Polygon2d upScalePolygon( - const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) -{ - Polygon2d transformed_polygon; - // upscale - for (size_t i = 0; i < polygon.outer().size(); i++) { - const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; - const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; - transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); - } - return transformed_polygon; -} - -inline geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) -{ - geometry_msgs::msg::Polygon polygon_msg; - geometry_msgs::msg::Point32 point_msg; - for (const auto & p : polygon.outer()) { - point_msg.x = p.x(); - point_msg.y = p.y(); - polygon_msg.points.push_back(point_msg); - } - return polygon_msg; -} } // namespace behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index e840b6f8a5..98df29d1c4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -15,8 +15,12 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#include +#include +#include +#include +#include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp index 4156f7df95..591ef8667b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -16,7 +16,6 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include -#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 7d5278c3cf..c86272cff1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -16,32 +16,18 @@ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ #include -#include -#include -#include -#include -#include -#include #include #include #include -#include #include -#include -#include - -#include -#include #include -#include #include #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; @@ -49,83 +35,15 @@ using TrajectoryPoints = std::vector; using geometry_msgs::msg::Quaternion; using TrajectoryPointWithIdx = std::pair; -inline TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) -{ - TrajectoryPoints tps; - for (const auto & p : path.points) { - TrajectoryPoint tp; - tp.pose = p.point.pose; - tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; - // since path point doesn't have acc for now - tp.acceleration_mps2 = 0; - tps.emplace_back(tp); - } - return tps; -} -inline PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) -{ - PathWithLaneId path; - for (const auto & p : trajectory) { - PathPointWithLaneId pp; - pp.point.pose = p.pose; - pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; - path.points.emplace_back(pp); - } - return path; -} - -inline Quaternion lerpOrientation( - const Quaternion & o_from, const Quaternion & o_to, const double ratio) -{ - tf2::Quaternion q_from, q_to; - tf2::fromMsg(o_from, q_from); - tf2::fromMsg(o_to, q_to); +TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path); +PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory); - const auto q_interpolated = q_from.slerp(q_to, ratio); - return tf2::toMsg(q_interpolated); -} +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio); //! smooth path point with lane id starts from ego position on path to the path end -inline bool smoothPath( +bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, - const std::shared_ptr & planner_data) -{ - const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; - const double v0 = planner_data->current_velocity->twist.linear.x; - const double a0 = planner_data->current_acceleration->accel.accel.linear.x; - const auto & external_v_limit = planner_data->external_velocity_limit; - const auto & smoother = planner_data->velocity_smoother_; - - auto trajectory = convertPathToTrajectoryPoints(in_path); - if (external_v_limit) { - motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( - 0, trajectory.size(), external_v_limit->max_velocity, trajectory); - } - const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); - - // 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, - 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, - planner_data->ego_nearest_yaw_threshold); - std::vector debug_trajectories; - // Clip trajectory from closest point - TrajectoryPoints clipped; - TrajectoryPoints traj_smoothed; - clipped.insert( - clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); - if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { - std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; - return false; - } - traj_smoothed.insert( - traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); - - out_path = convertTrajectoryPointsToPath(traj_smoothed); - return true; -} + const std::shared_ptr & planner_data); } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 6513a3c9a4..ceba86c27f 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -15,35 +15,17 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#include -#include -#include -#include +#include -#include #include -#include -#include #include -#include -#include #include #include -#include -#include #include -#include - -#include -#include -#include +#include #include -#include -#include -#include -#include -#include +#include #include #include @@ -56,11 +38,6 @@ namespace behavior_velocity_planner { -struct SearchRangeIndex -{ - size_t min_idx; - size_t max_idx; -}; struct DetectionRange { bool use_right = true; @@ -73,11 +50,6 @@ struct DetectionRange double right_overhang; double left_overhang; }; -struct PointWithSearchRangeIndex -{ - geometry_msgs::msg::Point point; - SearchRangeIndex index; -}; struct TrafficSignalStamped { @@ -85,75 +57,30 @@ struct TrafficSignalStamped autoware_perception_msgs::msg::TrafficSignal signal; }; -using geometry_msgs::msg::Pose; +using Pose = geometry_msgs::msg::Pose; +using Point2d = tier4_autoware_utils::Point2d; +using LineString2d = tier4_autoware_utils::LineString2d; +using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; -using autoware_auto_planning_msgs::msg::Trajectory; -using motion_utils::calcLongitudinalOffsetToSegment; -using motion_utils::calcSignedArcLength; -using motion_utils::validateNonEmpty; -using tier4_autoware_utils::calcAzimuthAngle; -using tier4_autoware_utils::calcDistance2d; -using tier4_autoware_utils::calcOffsetPose; -using tier4_autoware_utils::calcSquaredDistance2d; -using tier4_autoware_utils::createQuaternionFromYaw; -using tier4_autoware_utils::getPoint; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; namespace planning_utils { -template -size_t calcPointIndexFromSegmentIndex( - const std::vector & points, const geometry_msgs::msg::Point & point, const size_t seg_idx) -{ - const size_t prev_point_idx = seg_idx; - const size_t next_point_idx = seg_idx + 1; - - const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); - const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); - - if (prev_dist < next_dist) { - return prev_point_idx; - } - return next_point_idx; -} - -template size_t calcSegmentIndexFromPointIndex( - const std::vector & points, const geometry_msgs::msg::Point & point, const size_t idx) -{ - if (idx == 0) { - return 0; - } - if (idx == points.size() - 1) { - return idx - 1; - } - if (points.size() < 3) { - return 0; - } - - const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); - if (0 < offset_to_seg) { - return idx; - } - return idx - 1; -} - + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx); // create detection area from given range return false if creation failure bool createDetectionAreaPolygons( Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, const double min_velocity = 1.0); -PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio); -Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y); +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y); void extractClosePartition( const geometry_msgs::msg::Point position, const BasicPolygons2d & all_partitions, BasicPolygons2d & close_partition, const double distance_thresh = 30.0); @@ -167,12 +94,6 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } -geometry_msgs::msg::Pose transformRelCoordinate2D( - const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); -geometry_msgs::msg::Pose transformAbsCoordinate2D( - const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin); -SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id); - bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin); geometry_msgs::msg::Pose getAheadPose( const size_t start_idx, const double ahead_dist, diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp index 28ac40f086..5804c0cdaf 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/velocity_factor_interface.hpp @@ -16,13 +16,11 @@ #ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ #define BEHAVIOR_VELOCITY_PLANNER_COMMON__VELOCITY_FACTOR_INTERFACE_HPP_ -#include - #include #include +#include #include -#include #include #include @@ -44,19 +42,10 @@ class VelocityFactorInterface void init(const VelocityFactorType type) { type_ = type; } void reset() { velocity_factor_.type = VelocityFactor::UNKNOWN; } - template void set( - const T & points, const Pose & curr_pose, const Pose & stop_pose, - const VelocityFactorStatus status, const std::string detail = "") - { - const auto & curr_point = curr_pose.position; - const auto & stop_point = stop_pose.position; - velocity_factor_.type = type_; - velocity_factor_.pose = stop_pose; - velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); - velocity_factor_.status = status; - velocity_factor_.detail = detail; - } + const std::vector & points, + const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, + const std::string detail = ""); private: VelocityFactorType type_; diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp new file mode 100644 index 0000000000..6ba982294c --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -0,0 +1,264 @@ +// 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 +#include +#include +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ + +using tier4_autoware_utils::StopWatch; + +SceneModuleInterface::SceneModuleInterface( + const int64_t module_id, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +: module_id_(module_id), + activated_(false), + safe_(false), + distance_(std::numeric_limits::lowest()), + logger_(logger), + clock_(clock) +{ +} + +size_t SceneModuleInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold); +} + +SceneModuleManagerInterface::SceneModuleManagerInterface( + rclcpp::Node & node, [[maybe_unused]] const char * module_name) +: node_(node), clock_(node.get_clock()), logger_(node.get_logger()) +{ + const auto ns = std::string("~/debug/") + module_name; + pub_debug_ = node.create_publisher(ns, 1); + if (!node.has_parameter("is_publish_debug_path")) { + is_publish_debug_path_ = node.declare_parameter("is_publish_debug_path"); + } else { + is_publish_debug_path_ = node.get_parameter("is_publish_debug_path").as_bool(); + } + if (is_publish_debug_path_) { + pub_debug_path_ = node.create_publisher( + std::string("~/debug/path_with_lane_id/") + module_name, 1); + } + pub_virtual_wall_ = node.create_publisher( + std::string("~/virtual_wall/") + module_name, 5); + pub_velocity_factor_ = node.create_publisher( + std::string("/planning/velocity_factors/") + module_name, 1); + pub_stop_reason_ = + node.create_publisher("~/output/stop_reasons", 1); + pub_infrastructure_commands_ = + node.create_publisher( + "~/output/infrastructure_commands", 1); + + processing_time_publisher_ = std::make_shared(&node, "~/debug"); +} + +size_t SceneModuleManagerInterface::findEgoSegmentIndex( + const std::vector & points) const +{ + const auto & p = planner_data_; + return motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + points, p->current_odometry->pose, p->ego_nearest_dist_threshold, p->ego_nearest_yaw_threshold); +} + +void SceneModuleManagerInterface::updateSceneModuleInstances( + const std::shared_ptr & planner_data, + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + planner_data_ = planner_data; + + launchNewModules(path); + deleteExpiredModules(path); +} + +void SceneModuleManagerInterface::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + StopWatch stop_watch; + stop_watch.tic("Total"); + visualization_msgs::msg::MarkerArray debug_marker_array; + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + autoware_adapi_v1_msgs::msg::VelocityFactorArray velocity_factor_array; + stop_reason_array.header.frame_id = "map"; + stop_reason_array.header.stamp = clock_->now(); + velocity_factor_array.header.frame_id = "map"; + velocity_factor_array.header.stamp = clock_->now(); + + tier4_v2x_msgs::msg::InfrastructureCommandArray infrastructure_command_array; + infrastructure_command_array.stamp = clock_->now(); + + first_stop_path_point_index_ = static_cast(path->points.size()) - 1; + for (const auto & scene_module : scene_modules_) { + tier4_planning_msgs::msg::StopReason stop_reason; + scene_module->resetVelocityFactor(); + scene_module->setPlannerData(planner_data_); + scene_module->modifyPathVelocity(path, &stop_reason); + + // The velocity factor must be called after modifyPathVelocity. + const auto velocity_factor = scene_module->getVelocityFactor(); + if (velocity_factor.type != VelocityFactor::UNKNOWN) { + velocity_factor_array.factors.emplace_back(velocity_factor); + } + if (stop_reason.reason != "") { + stop_reason_array.stop_reasons.emplace_back(stop_reason); + } + + if (const auto command = scene_module->getInfrastructureCommand()) { + infrastructure_command_array.commands.push_back(*command); + } + + if (scene_module->getFirstStopPathPointIndex() < first_stop_path_point_index_) { + first_stop_path_point_index_ = scene_module->getFirstStopPathPointIndex(); + } + + for (const auto & marker : scene_module->createDebugMarkerArray().markers) { + debug_marker_array.markers.push_back(marker); + } + + virtual_wall_marker_creator_.add_virtual_walls(scene_module->createVirtualWalls()); + } + + if (!stop_reason_array.stop_reasons.empty()) { + pub_stop_reason_->publish(stop_reason_array); + } + pub_velocity_factor_->publish(velocity_factor_array); + pub_infrastructure_commands_->publish(infrastructure_command_array); + pub_debug_->publish(debug_marker_array); + if (is_publish_debug_path_) { + autoware_auto_planning_msgs::msg::PathWithLaneId debug_path; + debug_path.header = path->header; + debug_path.points = path->points; + pub_debug_path_->publish(debug_path); + } + pub_virtual_wall_->publish(virtual_wall_marker_creator_.create_markers(clock_->now())); + processing_time_publisher_->publish( + std::string(getModuleName()) + "/processing_time_ms", stop_watch.toc("Total")); +} + +void SceneModuleManagerInterface::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + unregisterModule(scene_module); + } + } +} + +void SceneModuleManagerInterface::registerModule( + const std::shared_ptr & scene_module) +{ + RCLCPP_INFO( + logger_, "register task: module = %s, id = %lu", getModuleName(), scene_module->getModuleId()); + registered_module_id_set_.emplace(scene_module->getModuleId()); + scene_modules_.insert(scene_module); +} + +void SceneModuleManagerInterface::unregisterModule( + const std::shared_ptr & scene_module) +{ + RCLCPP_INFO( + logger_, "unregister task: module = %s, id = %lu", getModuleName(), + scene_module->getModuleId()); + registered_module_id_set_.erase(scene_module->getModuleId()); + scene_modules_.erase(scene_module); +} + +SceneModuleManagerInterfaceWithRTC::SceneModuleManagerInterfaceWithRTC( + rclcpp::Node & node, const char * module_name, const bool enable_rtc) +: SceneModuleManagerInterface(node, module_name), rtc_interface_(&node, module_name, enable_rtc) +{ +} + +void SceneModuleManagerInterfaceWithRTC::plan( + autoware_auto_planning_msgs::msg::PathWithLaneId * path) +{ + setActivation(); + modifyPathVelocity(path); + sendRTC(path->header.stamp); +} + +void SceneModuleManagerInterfaceWithRTC::sendRTC(const Time & stamp) +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + updateRTCStatus(uuid, scene_module->isSafe(), scene_module->getDistance(), stamp); + } + publishRTCStatus(stamp); +} + +void SceneModuleManagerInterfaceWithRTC::setActivation() +{ + for (const auto & scene_module : scene_modules_) { + const UUID uuid = getUUID(scene_module->getModuleId()); + scene_module->setActivation(rtc_interface_.isActivated(uuid)); + } +} + +UUID SceneModuleManagerInterfaceWithRTC::getUUID(const int64_t & module_id) const +{ + if (map_uuid_.count(module_id) == 0) { + const UUID uuid; + return uuid; + } + return map_uuid_.at(module_id); +} + +void SceneModuleManagerInterfaceWithRTC::generateUUID(const int64_t & module_id) +{ + map_uuid_.insert({module_id, tier4_autoware_utils::generateUUID()}); +} + +void SceneModuleManagerInterfaceWithRTC::removeUUID(const int64_t & module_id) +{ + const auto result = map_uuid_.erase(module_id); + if (result == 0) { + RCLCPP_WARN_STREAM(logger_, "[removeUUID] module_id = " << module_id << " is not registered."); + } +} + +void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + const auto isModuleExpired = getModuleExpiredFunction(path); + + // Copy container to avoid iterator corruption + // due to scene_modules_.erase() in unregisterModule() + const auto copied_scene_modules = scene_modules_; + + for (const auto & scene_module : copied_scene_modules) { + if (isModuleExpired(scene_module)) { + removeRTCStatus(getUUID(scene_module->getModuleId())); + removeUUID(scene_module->getModuleId()); + unregisterModule(scene_module); + } + } +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp new file mode 100644 index 0000000000..05f634a81c --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -0,0 +1,138 @@ +// 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 +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include + +#include +#endif + +#include +#include +#include +#include + +namespace +{ +geometry_msgs::msg::Point operator+( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + geometry_msgs::msg::Point p; + p.x = p1.x + p2.x; + p.y = p1.y + p2.y; + p.z = p1.z + p2.z; + + return p; +} + +geometry_msgs::msg::Point operator*(const geometry_msgs::msg::Point & p, const double v) +{ + geometry_msgs::msg::Point multiplied_p; + multiplied_p.x = p.x * v; + multiplied_p.y = p.y * v; + multiplied_p.z = p.z * v; + + return multiplied_p; +} + +/* +geometry_msgs::msg::Point operator*(const double v, const geometry_msgs::msg::Point & p) +{ +return p * v; +} +*/ +} // namespace + +namespace behavior_velocity_planner::arc_lane_utils +{ + +double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) +{ + Eigen::Affine3d map2p1; + tf2::fromMsg(p1, map2p1); + const auto basecoords_p2 = map2p1.inverse() * Eigen::Vector3d(p2.x, p2.y, p2.z); + return basecoords_p2.x() >= 0 ? basecoords_p2.norm() : -basecoords_p2.norm(); +} + +// calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) + +boost::optional checkCollision( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, + const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) +{ + const double det = (p2.x - p1.x) * (p4.y - p3.y) - (p2.y - p1.y) * (p4.x - p3.x); + + if (det == 0.0) { + // collision is not one point. + return boost::none; + } + + const double t1 = ((p4.y - p3.y) * (p4.x - p1.x) - (p4.x - p3.x) * (p4.y - p1.y)) / det; + const double t2 = ((p2.x - p1.x) * (p4.y - p1.y) - (p2.y - p1.y) * (p4.x - p1.x)) / det; + + // check collision is outside the segment line + if (t1 < 0.0 || 1.0 < t1 || t2 < 0.0 || 1.0 < t2) { + return boost::none; + } + + return p1 * (1.0 - t1) + p2 * t1; +} + +boost::optional findOffsetSegment( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, + const double offset) +{ + if (offset >= 0) { + return findForwardOffsetSegment(path, index, offset); + } + + return findBackwardOffsetSegment(path, index, -offset); +} + +boost::optional createTargetPoint( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const size_t lane_id, const double margin, const double vehicle_offset) +{ + // Find collision segment + const auto collision_segment = findCollisionSegment(path, stop_line, lane_id); + if (!collision_segment) { + // No collision + return {}; + } + + // Calculate offset length from stop line + // Use '-' to make the positive direction is forward + const double offset_length = -(margin + vehicle_offset); + + // Find offset segment + const auto offset_segment = findOffsetSegment(path, *collision_segment, offset_length); + if (!offset_segment) { + // No enough path length + return {}; + } + + const auto target_pose = calcTargetPose(path, *offset_segment); + + const auto front_idx = offset_segment->first; + return std::make_pair(front_idx, target_pose); +} +} // namespace behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp new file mode 100644 index 0000000000..903cf5aab8 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -0,0 +1,66 @@ +// 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 + +#include + +#include +#include +namespace behavior_velocity_planner +{ + +Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) +{ + Polygon2d polygon; + + polygon.outer().push_back(left_line.front()); + + for (auto itr = right_line.begin(); itr != right_line.end(); ++itr) { + polygon.outer().push_back(*itr); + } + + for (auto itr = left_line.rbegin(); itr != left_line.rend(); ++itr) { + polygon.outer().push_back(*itr); + } + + bg::correct(polygon); + return polygon; +} + +Polygon2d upScalePolygon( + const geometry_msgs::msg::Point & position, const Polygon2d & polygon, const double scale) +{ + Polygon2d transformed_polygon; + // upscale + for (size_t i = 0; i < polygon.outer().size(); i++) { + const double upscale_x = (polygon.outer().at(i).x() - position.x) * scale + position.x; + const double upscale_y = (polygon.outer().at(i).y() - position.y) * scale + position.y; + transformed_polygon.outer().emplace_back(Point2d(upscale_x, upscale_y)); + } + return transformed_polygon; +} + +geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) +{ + geometry_msgs::msg::Polygon polygon_msg; + geometry_msgs::msg::Point32 point_msg; + for (const auto & p : polygon.outer()) { + point_msg.x = p.x(); + point_msg.y = p.y(); + polygon_msg.points.push_back(point_msg); + } + return polygon_msg; +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index 845b93576b..6ff2ef5317 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include +#include #include - namespace behavior_velocity_planner { namespace debug diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp index 34d4cf0efa..59c9e4861f 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -13,20 +13,10 @@ // limitations under the License. #include -#include -#include -#include #include +#include #include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include #include #include diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp new file mode 100644 index 0000000000..71cdd9650a --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -0,0 +1,121 @@ +// 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 +#include +#include +#include +#include + +#include +#include + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using TrajectoryPoints = std::vector; +using geometry_msgs::msg::Quaternion; +using TrajectoryPointWithIdx = std::pair; + +TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path) +{ + TrajectoryPoints tps; + for (const auto & p : path.points) { + TrajectoryPoint tp; + tp.pose = p.point.pose; + tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps; + // since path point doesn't have acc for now + tp.acceleration_mps2 = 0; + tps.emplace_back(tp); + } + return tps; +} +PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory) +{ + PathWithLaneId path; + for (const auto & p : trajectory) { + PathPointWithLaneId pp; + pp.point.pose = p.pose; + pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps; + path.points.emplace_back(pp); + } + return path; +} + +Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio) +{ + tf2::Quaternion q_from, q_to; + tf2::fromMsg(o_from, q_from); + tf2::fromMsg(o_to, q_to); + + const auto q_interpolated = q_from.slerp(q_to, ratio); + return tf2::toMsg(q_interpolated); +} + +//! smooth path point with lane id starts from ego position on path to the path end +bool smoothPath( + const PathWithLaneId & in_path, PathWithLaneId & out_path, + const std::shared_ptr & planner_data) +{ + const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose; + const double v0 = planner_data->current_velocity->twist.linear.x; + const double a0 = planner_data->current_acceleration->accel.accel.linear.x; + const auto & external_v_limit = planner_data->external_velocity_limit; + const auto & smoother = planner_data->velocity_smoother_; + + auto trajectory = convertPathToTrajectoryPoints(in_path); + if (external_v_limit) { + motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + 0, trajectory.size(), external_v_limit->max_velocity, trajectory); + } + const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory); + + // 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, + 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, + planner_data->ego_nearest_yaw_threshold); + std::vector debug_trajectories; + // Clip trajectory from closest point + TrajectoryPoints clipped; + TrajectoryPoints traj_smoothed; + clipped.insert( + clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end()); + if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) { + std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl; + return false; + } + traj_smoothed.insert( + traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); + + out_path = convertTrajectoryPointsToPath(traj_smoothed); + return true; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/behavior_velocity_planner_common/src/utilization/util.cpp index 3f8180956c..a3a5a8a169 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,23 +12,50 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include +#include +#include + +#include + +#include + +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif #include #include #include #include -namespace behavior_velocity_planner +namespace { -namespace planning_utils +size_t calcPointIndexFromSegmentIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t seg_idx) { -Point2d calculateOffsetPoint2d(const Pose & pose, const double offset_x, const double offset_y) -{ - return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); + const size_t prev_point_idx = seg_idx; + const size_t next_point_idx = seg_idx + 1; + + const double prev_dist = tier4_autoware_utils::calcDistance2d(point, points.at(prev_point_idx)); + const double next_dist = tier4_autoware_utils::calcDistance2d(point, points.at(next_point_idx)); + + if (prev_dist < next_dist) { + return prev_point_idx; + } + return next_point_idx; } +using autoware_auto_planning_msgs::msg::PathPoint; + PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, const double ratio) { auto lerp = [](const double a, const double b, const double t) { return a + t * (b - a); }; @@ -39,6 +66,71 @@ PathPoint getLerpPathPointWithLaneId(const PathPoint p0, const PathPoint p1, con return p; } +geometry_msgs::msg::Pose transformRelCoordinate2D( + const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) +{ + // translation + geometry_msgs::msg::Point trans_p; + trans_p.x = target.position.x - origin.position.x; + trans_p.y = target.position.y - origin.position.y; + + // rotation (use inverse matrix of rotation) + double yaw = tf2::getYaw(origin.orientation); + + geometry_msgs::msg::Pose res; + res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); + res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); + res.position.z = target.position.z - origin.position.z; + res.orientation = + tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); + + return res; +} + +} // namespace + +namespace behavior_velocity_planner +{ +namespace planning_utils +{ +using autoware_auto_planning_msgs::msg::PathPoint; +using motion_utils::calcLongitudinalOffsetToSegment; +using motion_utils::calcSignedArcLength; +using motion_utils::validateNonEmpty; +using tier4_autoware_utils::calcAzimuthAngle; +using tier4_autoware_utils::calcDistance2d; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::calcSquaredDistance2d; +using tier4_autoware_utils::createQuaternionFromYaw; +using tier4_autoware_utils::getPoint; + +size_t calcSegmentIndexFromPointIndex( + const std::vector & points, + const geometry_msgs::msg::Point & point, const size_t idx) +{ + if (idx == 0) { + return 0; + } + if (idx == points.size() - 1) { + return idx - 1; + } + if (points.size() < 3) { + return 0; + } + + const double offset_to_seg = motion_utils::calcLongitudinalOffsetToSegment(points, idx, point); + if (0 < offset_to_seg) { + return idx; + } + return idx - 1; +} + +Point2d calculateOffsetPoint2d( + const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y) +{ + return to_bg2d(calcOffsetPose(pose, offset_x, offset_y, 0.0)); +} + bool createDetectionAreaPolygons( Polygons2d & da_polys, const PathWithLaneId & path, const geometry_msgs::msg::Pose & target_pose, const size_t target_seg_idx, const DetectionRange & da_range, const double obstacle_vel_mps, @@ -168,30 +260,6 @@ void getAllPartitionLanelets(const lanelet::LaneletMapConstPtr ll, BasicPolygons } } -SearchRangeIndex getPathIndexRangeIncludeLaneId(const PathWithLaneId & path, const int64_t lane_id) -{ - /** - * @brief find path index range include given lane_id - * |<-min_idx |<-max_idx - * ------|oooooooooooooooo|------- - */ - SearchRangeIndex search_range = {0, path.points.size() - 1}; - bool found_first_idx = false; - for (size_t i = 0; i < path.points.size(); i++) { - const auto & p = path.points.at(i); - for (const auto & id : p.lane_ids) { - if (id == lane_id) { - if (!found_first_idx) { - search_range.min_idx = i; - found_first_idx = true; - } - search_range.max_idx = i; - } - } - } - return search_range; -} - void setVelocityFromIndex(const size_t begin_idx, const double vel, PathWithLaneId * input) { for (size_t i = begin_idx; i < input->points.size(); ++i) { @@ -229,7 +297,7 @@ void insertVelocity( bool isAheadOf(const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) { - geometry_msgs::msg::Pose p = planning_utils::transformRelCoordinate2D(target, origin); + geometry_msgs::msg::Pose p = transformRelCoordinate2D(target, origin); const bool is_target_ahead = (p.position.x > 0.0); return is_target_ahead; } @@ -314,47 +382,6 @@ lanelet::ConstLanelet generatePathLanelet( return lanelet::Lanelet(lanelet::InvalId, left, right); } -geometry_msgs::msg::Pose transformRelCoordinate2D( - const geometry_msgs::msg::Pose & target, const geometry_msgs::msg::Pose & origin) -{ - // translation - geometry_msgs::msg::Point trans_p; - trans_p.x = target.position.x - origin.position.x; - trans_p.y = target.position.y - origin.position.y; - - // rotation (use inverse matrix of rotation) - double yaw = tf2::getYaw(origin.orientation); - - geometry_msgs::msg::Pose res; - res.position.x = (std::cos(yaw) * trans_p.x) + (std::sin(yaw) * trans_p.y); - res.position.y = ((-1.0) * std::sin(yaw) * trans_p.x) + (std::cos(yaw) * trans_p.y); - res.position.z = target.position.z - origin.position.z; - res.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(target.orientation) - yaw); - - return res; -} - -geometry_msgs::msg::Pose transformAbsCoordinate2D( - const geometry_msgs::msg::Pose & relative, const geometry_msgs::msg::Pose & origin) -{ - // rotation - geometry_msgs::msg::Point rot_p; - double yaw = tf2::getYaw(origin.orientation); - rot_p.x = (std::cos(yaw) * relative.position.x) + (-std::sin(yaw) * relative.position.y); - rot_p.y = (std::sin(yaw) * relative.position.x) + (std::cos(yaw) * relative.position.y); - - // translation - geometry_msgs::msg::Pose absolute; - absolute.position.x = rot_p.x + origin.position.x; - absolute.position.y = rot_p.y + origin.position.y; - absolute.position.z = relative.position.z + origin.position.z; - absolute.orientation = - tier4_autoware_utils::createQuaternionFromYaw(tf2::getYaw(relative.orientation) + yaw); - - return absolute; -} - double calcJudgeLineDistWithAccLimit( const double velocity, const double max_stop_acceleration, const double delay_response_time) { diff --git a/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp new file mode 100644 index 0000000000..35c4bc00d8 --- /dev/null +++ b/planning/behavior_velocity_planner_common/src/velocity_factor_interface.cpp @@ -0,0 +1,33 @@ +// 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 +#include +namespace behavior_velocity_planner +{ +void VelocityFactorInterface::set( + const std::vector & points, + const Pose & curr_pose, const Pose & stop_pose, const VelocityFactorStatus status, + const std::string detail) +{ + const auto & curr_point = curr_pose.position; + const auto & stop_point = stop_pose.position; + velocity_factor_.type = type_; + velocity_factor_.pose = stop_pose; + velocity_factor_.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); + velocity_factor_.status = status; + velocity_factor_.detail = detail; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index 60a0ff878b..c4fdb83e00 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -16,7 +16,6 @@ #include #include -#include #include diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/behavior_velocity_run_out_module/CMakeLists.txt index 89745a1806..21758c4d01 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/behavior_velocity_run_out_module/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/state_machine.cpp src/utils.cpp + src/path_utils.cpp ) ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/behavior_velocity_run_out_module/src/debug.cpp index 90f69e0faa..c0d026b5ff 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/behavior_velocity_run_out_module/src/debug.cpp @@ -17,6 +17,7 @@ #include "scene.hpp" #include +#include #include using tier4_autoware_utils::appendMarkerArray; @@ -75,7 +76,6 @@ RunOutDebug::RunOutDebug(rclcpp::Node & node) : node_(node) pub_debug_values_ = node.create_publisher("~/debug/run_out/debug_values", 1); pub_accel_reason_ = node.create_publisher("~/debug/run_out/accel_reason", 1); - pub_debug_trajectory_ = node.create_publisher("~/debug/run_out/trajectory", 1); pub_debug_pointcloud_ = node.create_publisher( "~/debug/run_out/filtered_pointcloud", rclcpp::SensorDataQoS().keep_last(1)); } @@ -305,11 +305,6 @@ void RunOutDebug::publishDebugValue() pub_accel_reason_->publish(accel_reason); } -void RunOutDebug::publishDebugTrajectory(const Trajectory & trajectory) -{ - pub_debug_trajectory_->publish(trajectory); -} - void RunOutDebug::publishFilteredPointCloud( const pcl::PointCloud & pointcloud, const std_msgs::msg::Header header) { diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/behavior_velocity_run_out_module/src/debug.hpp index 0dcd3099b6..91656d542e 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/behavior_velocity_run_out_module/src/debug.hpp @@ -26,7 +26,6 @@ #include namespace behavior_velocity_planner { -using autoware_auto_planning_msgs::msg::Trajectory; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Int32Stamped; @@ -113,7 +112,6 @@ class RunOutDebug const double travel_time, const geometry_msgs::msg::Pose pose, const float lateral_offset); void setAccelReason(const AccelReason & accel_reason); void publishDebugValue(); - void publishDebugTrajectory(const Trajectory & trajectory); void publishFilteredPointCloud(const PointCloud2 & pointcloud); void publishFilteredPointCloud( const pcl::PointCloud & pointcloud, const std_msgs::msg::Header header); @@ -130,7 +128,6 @@ class RunOutDebug rclcpp::Node & node_; rclcpp::Publisher::SharedPtr pub_debug_values_; rclcpp::Publisher::SharedPtr pub_accel_reason_; - rclcpp::Publisher::SharedPtr pub_debug_trajectory_; rclcpp::Publisher::SharedPtr pub_debug_pointcloud_; std::vector collision_points_; std::vector nearest_collision_point_; diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index 728e0fbad9..16013efc5f 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -14,11 +14,15 @@ #include "dynamic_obstacle.hpp" +#include #include #include #include #include +#include +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 7eece8fac2..5217193c96 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -36,6 +36,8 @@ #include #endif +#include + #include #include diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/behavior_velocity_run_out_module/src/path_utils.cpp new file mode 100644 index 0000000000..1cec202976 --- /dev/null +++ b/planning/behavior_velocity_run_out_module/src/path_utils.cpp @@ -0,0 +1,39 @@ +// 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 "path_utils.hpp" + +#include +namespace behavior_velocity_planner::run_out_utils +{ +geometry_msgs::msg::Point findLongitudinalNearestPoint( + const std::vector & points, + const geometry_msgs::msg::Point & src_point, + const std::vector & target_points) +{ + float min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point min_dist_point{}; + + for (const auto & p : target_points) { + const float dist = motion_utils::calcSignedArcLength(points, src_point, p); + if (dist < min_dist) { + min_dist = dist; + min_dist_point = p; + } + } + + return min_dist_point; +} + +} // namespace behavior_velocity_planner::run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/behavior_velocity_run_out_module/src/path_utils.hpp index 11ed2b7282..92aca946c1 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/path_utils.hpp @@ -15,6 +15,9 @@ #ifndef PATH_UTILS_HPP_ #define PATH_UTILS_HPP_ +#include +#include + #include #include #include @@ -27,24 +30,10 @@ namespace behavior_velocity_planner namespace run_out_utils { -template geometry_msgs::msg::Point findLongitudinalNearestPoint( - const T & points, const geometry_msgs::msg::Point & src_point, - const std::vector & target_points) -{ - float min_dist = std::numeric_limits::max(); - geometry_msgs::msg::Point min_dist_point{}; - - for (const auto & p : target_points) { - const float dist = motion_utils::calcSignedArcLength(points, src_point, p); - if (dist < min_dist) { - min_dist = dist; - min_dist_point = p; - } - } - - return min_dist_point; -} + const std::vector & points, + const geometry_msgs::msg::Point & src_point, + const std::vector & target_points); } // namespace run_out_utils } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/behavior_velocity_run_out_module/src/scene.cpp index b062bf9e31..b91d1d1fe1 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/behavior_velocity_run_out_module/src/scene.cpp @@ -19,6 +19,10 @@ #include #include #include +#include +#include + +#include #include #include diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index 71ab07922b..61396da250 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -17,9 +17,22 @@ #include #include +#include +#include +#include + +#include + #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include +#include namespace behavior_velocity_planner { namespace run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index 74bc2489d7..8fc3d48de4 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -19,6 +19,8 @@ #include #include +#include +#include #include #include @@ -31,6 +33,8 @@ namespace run_out_utils namespace bg = boost::geometry; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 3f9a60d1d2..7409c24e7e 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -15,8 +15,10 @@ #include "scene.hpp" #include +#include #include #include +#include #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index dacc99d55b..5949357c90 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -14,6 +14,8 @@ #include "manager.hpp" +#include +#include #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index 1a797adf2c..34de722055 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -18,6 +18,7 @@ #include "util.hpp" #include +#include #include #include diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index c815dab29a..21b283a397 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -24,6 +24,8 @@ #include #include +#include + #include #include #include diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index 8d7de00635..f63bdca506 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include +#include #include - #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_template_module/CMakeLists.txt b/planning/behavior_velocity_template_module/CMakeLists.txt new file mode 100644 index 0000000000..e0beb42daf --- /dev/null +++ b/planning/behavior_velocity_template_module/CMakeLists.txt @@ -0,0 +1,13 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_velocity_template_module) + +find_package(autoware_cmake REQUIRED) +autoware_package() +pluginlib_export_plugin_description_file(behavior_velocity_planner plugins.xml) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/manager.cpp + src/scene.cpp +) + +ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_velocity_template_module/README.md b/planning/behavior_velocity_template_module/README.md new file mode 100644 index 0000000000..1995b3e040 --- /dev/null +++ b/planning/behavior_velocity_template_module/README.md @@ -0,0 +1,85 @@ +## Template + +A template for behavior velocity modules based on the behavior_velocity_speed_bump_module. + +# Autoware Behavior Velocity Module Template + +## `Scene` + +### `TemplateModule` Class + +The `TemplateModule` class serves as a foundation for creating a scene module within the Autoware behavior velocity planner. It defines the core methods and functionality needed for the module's behavior. You should replace the placeholder code with actual implementations tailored to your specific behavior velocity module. + +#### Constructor + +- The constructor for `TemplateModule` takes the essential parameters to create a module: `const int64_t module_id`, `const rclcpp::Logger & logger`, and `const rclcpp::Clock::SharedPtr clock`. These parameters are supplied by the `TemplateModuleManager` when registering a new module. Other parameters can be added to the constructor, if required by your specific module implementation. + +#### `modifyPathVelocity` Method + +- This method, defined in the `TemplateModule` class, is expected to modify the velocity of the input path based on certain conditions. In the provided code, it logs an informational message once when the template module is executing. +- The specific logic for velocity modification should be implemented in this method based on the module's requirements. + +#### `createDebugMarkerArray` Method + +- This method, also defined in the `TemplateModule` class, is responsible for creating a visualization of debug markers and returning them as a `visualization_msgs::msg::MarkerArray`. In the provided code, it returns an empty `MarkerArray`. +- You should implement the logic to generate debug markers specific to your module's functionality. + +#### `createVirtualWalls` Method + +- The `createVirtualWalls` method creates virtual walls for the scene and returns them as `motion_utils::VirtualWalls`. In the provided code, it returns an empty `VirtualWalls` object. +- You should implement the logic to create virtual walls based on your module's requirements. + +## `Manager` + +The managing of your modules is defined in manager.hpp and manager.cpp. The managing is handled by two classes: + +- The `TemplateModuleManager` class defines the core logic for managing and launching the behavior_velocity_template scenes (defined in behavior_velocity_template_module/src/scene.cpp/hpp). It inherits essential manager attributes from its parent class `SceneModuleManagerInterface`. +- The `TemplateModulePlugin` class provides a way to integrate the `TemplateModuleManager` into the logic of the Behavior Velocity Planner. + +### `TemplateModuleManager` Class + +#### Constructor `TemplateModuleManager` + +- This is the constructor of the `TemplateModuleManager` class, and it takes an `rclcpp::Node` reference as a parameter. +- It initializes a member variable `dummy_parameter` to 0.0. + +#### `getModuleName()` Method + +- This method is an override of a virtual method from the `SceneModuleManagerInterface` class. +- It returns a pointer to a constant character string, which is the name of the module. In this case, it returns "template" as the module name. + +#### `launchNewModules()` Method + +- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- It is responsible for launching new modules based on the provided path information (PathWithLaneId). The implementation of this method involves initializing and configuring modules specific to your behavior velocity planner by using the `TemplateModule` class. +- In the provided source code, it initializes a `module_id` to 0 and checks if a module with the same ID is already registered. If not, it registers a new `TemplateModule` with the module ID. Note that each module managed by the `TemplateModuleManager` should have a unique ID. The template code registers a single module, so the `module_id` is set as 0 for simplicity. + +#### `getModuleExpiredFunction()` Method + +- This is a private method that takes an argument of type `autoware_auto_planning_msgs::msg::PathWithLaneId`. +- It returns a `std::function&)>`. This function is used by the behavior velocity planner to determine whether a particular module has expired or not based on the given path. +- The implementation of this method is expected to return a function that can be used to check the expiration status of modules. + +Please note that the specific functionality of the methods `launchNewModules()` and `getModuleExpiredFunction()` would depend on the details of your behavior velocity modules and how they are intended to be managed within the Autoware system. You would need to implement these methods according to your module's requirements. + +### `TemplateModulePlugin` Class + +#### `TemplateModulePlugin` Class + +- This class inherits from `PluginWrapper`. It essentially wraps your `TemplateModuleManager` class within a plugin, which can be loaded and managed dynamically. + +## `Example Usage` + +In the following example, we take each point of the path, and multiply it by 2. Essentially duplicating the speed. Note that the velocity smoother will further modify the path speed after all the behavior velocity modules are executed. + +```cpp +bool TemplateModule::modifyPathVelocity( + [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + for (auto & p : path->points) { + p.point.longitudinal_velocity_mps *= 2.0; + } + + return false; +} +``` diff --git a/planning/behavior_velocity_template_module/config/template.param.yaml b/planning/behavior_velocity_template_module/config/template.param.yaml new file mode 100644 index 0000000000..7045eb28e9 --- /dev/null +++ b/planning/behavior_velocity_template_module/config/template.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + template: + dummy: 1.2 diff --git a/planning/behavior_velocity_template_module/package.xml b/planning/behavior_velocity_template_module/package.xml new file mode 100644 index 0000000000..93a1cb4042 --- /dev/null +++ b/planning/behavior_velocity_template_module/package.xml @@ -0,0 +1,37 @@ + + + + behavior_velocity_template_module + 0.1.0 + The behavior_velocity_template_module package + + Daniel Sanchez + + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + eigen3_cmake_module + + autoware_auto_planning_msgs + behavior_velocity_planner_common + eigen + geometry_msgs + lanelet2_extension + libboost-dev + motion_utils + pluginlib + rclcpp + route_handler + sensor_msgs + tf2 + tier4_autoware_utils + visualization_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_velocity_template_module/plugins.xml b/planning/behavior_velocity_template_module/plugins.xml new file mode 100644 index 0000000000..3560c84e6a --- /dev/null +++ b/planning/behavior_velocity_template_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_template_module/src/manager.cpp b/planning/behavior_velocity_template_module/src/manager.cpp new file mode 100644 index 0000000000..22763f44af --- /dev/null +++ b/planning/behavior_velocity_template_module/src/manager.cpp @@ -0,0 +1,63 @@ +// 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 "manager.hpp" + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +using tier4_autoware_utils::getOrDeclareParameter; + +TemplateModuleManager::TemplateModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + std::string ns(getModuleName()); + dummy_parameter = getOrDeclareParameter(node, ns + ".dummy"); +} + +void TemplateModuleManager::launchNewModules( + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + int64_t module_id = 0; + if (!isModuleRegistered(module_id)) { + registerModule( + std::make_shared(module_id, logger_.get_child(getModuleName()), clock_)); + } +} + +std::function &)> +TemplateModuleManager::getModuleExpiredFunction( + [[maybe_unused]] const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return []([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { + return false; + }; +} + +} // namespace behavior_velocity_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_velocity_planner::TemplateModulePlugin, behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_template_module/src/manager.hpp b/planning/behavior_velocity_template_module/src/manager.hpp new file mode 100644 index 0000000000..ef4e1f00ca --- /dev/null +++ b/planning/behavior_velocity_template_module/src/manager.hpp @@ -0,0 +1,93 @@ +// 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 MANAGER_HPP_ +#define MANAGER_HPP_ + +#include "scene.hpp" + +#include +#include +#include +#include + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +/** + * @brief Constructor for the TemplateModuleManager class. + * + * Initializes a TemplateModuleManager instance with the provided ROS node, setting up essential + * parameters. + * + * @param node A reference to the ROS node. + */ +class TemplateModuleManager : public SceneModuleManagerInterface +{ +public: + explicit TemplateModuleManager(rclcpp::Node & node); + + /** + * @brief Get the name of the module. + * + * This method returns a constant character string representing the name of the module, which in + * this case is "template." + * + * @return A pointer to a constant character string containing the module name. + */ + const char * getModuleName() override { return "template"; } + +private: + double dummy_parameter{0.0}; + + /** + * @brief Launch new modules based on the provided path. + * + * This method is responsible for launching new modules based on the information provided in the + * given path. + * + * @param path The path with lane ID information to determine module launch. + */ + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + /** + * @brief Get a function to check module expiration. + * + * This method returns a function that can be used to determine whether a specific module has + * expired based on the given path information. + * + * @param path The path with lane ID information for module expiration check. + * @return A function for checking module expiration. + */ + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; +}; + +/** + * @brief Plugin class for TemplateModuleManager. + * + * The TemplateModulePlugin class is used to integrate the TemplateModuleManager into the Behavior + * Velocity Planner. + */ +class TemplateModulePlugin : public PluginWrapper +{ +}; + +} // namespace behavior_velocity_planner + +#endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_template_module/src/scene.cpp b/planning/behavior_velocity_template_module/src/scene.cpp new file mode 100644 index 0000000000..558f67b498 --- /dev/null +++ b/planning/behavior_velocity_template_module/src/scene.cpp @@ -0,0 +1,52 @@ +// 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 "scene.hpp" + +#include "motion_utils/trajectory/trajectory.hpp" +// #include "tier4_autoware_utils/tier4_autoware_utils.hpp" + +#include + +#include + +namespace behavior_velocity_planner +{ + +TemplateModule::TemplateModule( + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock) +{ +} + +visualization_msgs::msg::MarkerArray TemplateModule::createDebugMarkerArray() +{ + visualization_msgs::msg::MarkerArray ma; + return ma; +}; + +motion_utils::VirtualWalls TemplateModule::createVirtualWalls() +{ + motion_utils::VirtualWalls vw; + return vw; +} + +bool TemplateModule::modifyPathVelocity( + [[maybe_unused]] PathWithLaneId * path, [[maybe_unused]] StopReason * stop_reason) +{ + RCLCPP_INFO_ONCE(logger_, "Template Module is executing!"); + return false; +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_template_module/src/scene.hpp b/planning/behavior_velocity_template_module/src/scene.hpp new file mode 100644 index 0000000000..fab0b23efb --- /dev/null +++ b/planning/behavior_velocity_template_module/src/scene.hpp @@ -0,0 +1,69 @@ +// 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 SCENE_HPP_ +#define SCENE_HPP_ + +#include +#include + +#include +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::PathWithLaneId; + +class TemplateModule : public SceneModuleInterface +{ +public: + TemplateModule( + const int64_t module_id, const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + + /** + * @brief Modify the velocity of path points. + * + * This method is responsible for adjusting the velocity of each point in the input path based on + * specific criteria. + * + * @param path A pointer to the path containing points to be modified. + * @param stop_reason A pointer to the stop reason data. + * @return [bool] wether the path velocity was modified or not. + */ + bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; + + /** + * @brief Create a visualization of debug markers. + * + * This method is responsible for generating a visualization of debug markers and returning them + * as a `visualization_msgs::msg::MarkerArray`. + * + * @return A `visualization_msgs::msg::MarkerArray` containing debug markers. + */ + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + + /** + * @brief Create virtual walls for the scene. + * + * This method is responsible for generating virtual walls for the scene and returning them as a + * `motion_utils::VirtualWalls` object. + * + * @return A `motion_utils::VirtualWalls` object representing virtual walls in the scene. + */ + motion_utils::VirtualWalls createVirtualWalls() override; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index 6afd5381ce..9a0ba4f37c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include +#include #include - #ifdef ROS_DISTRO_GALACTIC #include #else diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index f035544b81..b943feddc1 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "manager.hpp" +#include #include #include @@ -24,7 +25,6 @@ #include #include #include - namespace behavior_velocity_planner { using lanelet::TrafficLight; diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index e0633926e3..7c4e947a42 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include // To be replaced by std::optional in C++17 #include diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp index e5cb4dcbc4..83b5e6317a 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/debug.cpp @@ -16,8 +16,8 @@ #include #include +#include #include - using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; diff --git a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 3fe6c19a5f..5310fae78c 100644 --- a/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,6 +14,7 @@ #include "manager.hpp" +#include #include #include diff --git a/planning/behavior_velocity_walkway_module/src/debug.cpp b/planning/behavior_velocity_walkway_module/src/debug.cpp index 51db3c204f..8814fa8271 100644 --- a/planning/behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/behavior_velocity_walkway_module/src/debug.cpp @@ -15,8 +15,10 @@ #include "scene_walkway.hpp" #include -#include -#include +#include +#include +#include +#include #include diff --git a/planning/behavior_velocity_walkway_module/src/manager.cpp b/planning/behavior_velocity_walkway_module/src/manager.cpp index f9c222f662..8995c2362a 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/behavior_velocity_walkway_module/src/manager.cpp @@ -45,7 +45,7 @@ void WalkwayModuleManager::launchNewModules(const PathWithLaneId & path) { const auto rh = planner_data_->route_handler_; - const auto launch = [this, &path](const auto & lanelet, const auto use_regulatory_element) { + const auto launch = [this, &path](const auto & lanelet, const auto & use_regulatory_element) { const auto attribute = lanelet.attributeOr(lanelet::AttributeNamesString::Subtype, std::string("")); if (attribute != lanelet::AttributeValueString::Walkway) { diff --git a/planning/behavior_velocity_walkway_module/src/manager.hpp b/planning/behavior_velocity_walkway_module/src/manager.hpp index 555e3a73db..453fcdb40f 100644 --- a/planning/behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/behavior_velocity_walkway_module/src/manager.hpp @@ -50,8 +50,6 @@ class WalkwayModuleManager : public SceneModuleManagerInterface std::function &)> getModuleExpiredFunction( const PathWithLaneId & path) override; - - std::optional opt_use_regulatory_element_{std::nullopt}; }; class WalkwayModulePlugin : public PluginWrapper diff --git a/planning/mission_planner/config/mission_planner.param.yaml b/planning/mission_planner/config/mission_planner.param.yaml index 98c28344ea..9b7dcffbc6 100644 --- a/planning/mission_planner/config/mission_planner.param.yaml +++ b/planning/mission_planner/config/mission_planner.param.yaml @@ -9,3 +9,4 @@ reroute_time_threshold: 10.0 minimum_reroute_length: 30.0 consider_no_drivable_lanes: false # This flag is for considering no_drivable_lanes in planning or not. + check_footprint_inside_lanes: true diff --git a/planning/mission_planner/schema/mission_planner_nodes.schema.json b/planning/mission_planner/schema/mission_planner_nodes.schema.json new file mode 100644 index 0000000000..3d0bdc41c5 --- /dev/null +++ b/planning/mission_planner/schema/mission_planner_nodes.schema.json @@ -0,0 +1,83 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Mission Planner nodes", + "type": "object", + "definitions": { + "mission_planner_nodes": { + "type": "object", + "properties": { + "map_frame": { + "type": "string", + "description": "The frame name for map ", + "default": "map" + }, + "arrival_check_angle_deg": { + "type": "number", + "description": "Angle threshold for goal check ", + "default": "45.0" + }, + "arrival_check_distance": { + "type": "number", + "description": "Distance threshold for goal check", + "default": "1.0", + "minimum": 0.0 + }, + "arrival_check_duration": { + "type": "number", + "description": "Duration threshold for goal check", + "default": "1.0", + "minimum": 0.0 + }, + "goal_angle_threshold_deg": { + "type": "number", + "description": "Max goal pose angle for goal approve", + "default": "45.0" + }, + "enable_correct_goal_pose": { + "type": "boolean", + "description": "Enabling correction of goal pose according to the closest lanelet orientation", + "default": "false" + }, + "reroute_time_threshold": { + "type": "number", + "description": "If the time to the rerouting point at the current velocity is greater than this threshold, rerouting is possible", + "default": "10.0", + "minimum": 0.0 + }, + "minimum_reroute_length": { + "type": "number", + "description": "Minimum Length for publishing a new route", + "default": "30.0" + }, + "consider_no_drivable_lanes": { + "type": "boolean", + "description": "This flag is for considering no_drivable_lanes in planning or not", + "default": "false" + } + }, + "required": [ + "map_frame", + "arrival_check_angle_deg", + "arrival_check_distance", + "arrival_check_duration", + "goal_angle_threshold_deg", + "enable_correct_goal_pose", + "reroute_time_threshold", + "minimum_reroute_length", + "consider_no_drivable_lanes" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/mission_planner_nodes" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index 979800aea0..673519b6f7 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -152,6 +152,8 @@ void DefaultPlanner::initialize_common(rclcpp::Node * node) param_.goal_angle_threshold_deg = node_->declare_parameter("goal_angle_threshold_deg"); param_.enable_correct_goal_pose = node_->declare_parameter("enable_correct_goal_pose"); param_.consider_no_drivable_lanes = node_->declare_parameter("consider_no_drivable_lanes"); + param_.check_footprint_inside_lanes = + node_->declare_parameter("check_footprint_inside_lanes"); } void DefaultPlanner::initialize(rclcpp::Node * node) @@ -349,6 +351,7 @@ bool DefaultPlanner::is_goal_valid( // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( + param_.check_footprint_inside_lanes && !check_goal_footprint( closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && !is_in_parking_lot( diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index cf5a19443d..251d0db533 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -38,6 +38,7 @@ struct DefaultPlannerParameters double goal_angle_threshold_deg; bool enable_correct_goal_pose; bool consider_no_drivable_lanes; + bool check_footprint_inside_lanes; }; class DefaultPlanner : public mission_planner::PlannerPlugin diff --git a/planning/mission_planner/src/mission_planner/mission_planner.cpp b/planning/mission_planner/src/mission_planner/mission_planner.cpp index 08da1260b0..7e071e3b92 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.cpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.cpp @@ -24,6 +24,8 @@ #include #include +#include + #include #include #include @@ -97,12 +99,14 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) rclcpp::QoS(1), std::bind(&MissionPlanner::on_reroute_availability, this, std::placeholders::_1)); - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); + const auto durable_qos = rclcpp::QoS(1).transient_local(); sub_vector_map_ = create_subscription( - "input/vector_map", qos_transient_local, + "input/vector_map", durable_qos, std::bind(&MissionPlanner::on_map, this, std::placeholders::_1)); + sub_modified_goal_ = create_subscription( + "input/modified_goal", durable_qos, + std::bind(&MissionPlanner::on_modified_goal, this, std::placeholders::_1)); - const auto durable_qos = rclcpp::QoS(1).transient_local(); pub_marker_ = create_publisher("debug/route_marker", durable_qos); const auto adaptor = component_interface_utils::NodeAdaptor(this); @@ -117,9 +121,30 @@ MissionPlanner::MissionPlanner(const rclcpp::NodeOptions & options) adaptor.init_srv(srv_change_route_points_, this, &MissionPlanner::on_change_route_points); adaptor.init_srv(srv_set_mrm_route_, this, &MissionPlanner::on_set_mrm_route); adaptor.init_srv(srv_clear_mrm_route_, this, &MissionPlanner::on_clear_mrm_route); - adaptor.init_sub(sub_modified_goal_, this, &MissionPlanner::on_modified_goal); + // Route state will be published when the node gets ready for route api after initialization, + // otherwise the mission planner rejects the request for the API. + data_check_timer_ = create_wall_timer( + std::chrono::milliseconds(100), std::bind(&MissionPlanner::checkInitialization, this)); +} + +void MissionPlanner::checkInitialization() +{ + if (!planner_->ready()) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting lanelet map... Route API is not ready."); + return; + } + if (!odometry_) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "waiting odometry... Route API is not ready."); + return; + } + + // All data is ready. Now API is available. + RCLCPP_INFO(get_logger(), "Route API is ready."); change_state(RouteState::Message::UNSET); + data_check_timer_->cancel(); // stop timer callback } void MissionPlanner::on_odometry(const Odometry::ConstSharedPtr msg) diff --git a/planning/mission_planner/src/mission_planner/mission_planner.hpp b/planning/mission_planner/src/mission_planner/mission_planner.hpp index fd1b317970..f7af76f43b 100644 --- a/planning/mission_planner/src/mission_planner/mission_planner.hpp +++ b/planning/mission_planner/src/mission_planner/mission_planner.hpp @@ -75,12 +75,18 @@ class MissionPlanner : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_odometry_; rclcpp::Subscription::SharedPtr sub_vector_map_; rclcpp::Subscription::SharedPtr sub_reroute_availability_; + rclcpp::Subscription::SharedPtr sub_modified_goal_; + Odometry::ConstSharedPtr odometry_; HADMapBin::ConstSharedPtr map_ptr_; RerouteAvailability::ConstSharedPtr reroute_availability_; void on_odometry(const Odometry::ConstSharedPtr msg); void on_map(const HADMapBin::ConstSharedPtr msg); void on_reroute_availability(const RerouteAvailability::ConstSharedPtr msg); + void on_modified_goal(const PoseWithUuidStamped::ConstSharedPtr msg); + + rclcpp::TimerBase::SharedPtr data_check_timer_; + void checkInitialization(); rclcpp::Publisher::SharedPtr pub_marker_; void clear_route(); @@ -128,8 +134,6 @@ class MissionPlanner : public rclcpp::Node const ClearMrmRoute::Service::Request::SharedPtr req, const ClearMrmRoute::Service::Response::SharedPtr res); - component_interface_utils::Subscription::SharedPtr sub_modified_goal_; - void on_modified_goal(const ModifiedGoal::Message::ConstSharedPtr msg); void on_change_route( const SetRoute::Service::Request::SharedPtr req, const SetRoute::Service::Response::SharedPtr res); diff --git a/planning/motion_velocity_smoother/README.md b/planning/motion_velocity_smoother/README.md index d52a6431c4..1ff7f5982b 100644 --- a/planning/motion_velocity_smoother/README.md +++ b/planning/motion_velocity_smoother/README.md @@ -140,6 +140,7 @@ After the optimization, a resampling called `post resampling` is performed befor | Name | Type | Description | Default value | | :------------------------------------- | :------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| `enable_lateral_acc_limit` | `bool` | To toggle the lateral acceleration filter on and off. You can switch it dynamically at runtime. | true | | `max_lateral_accel` | `double` | Max lateral acceleration limit [m/ss] | 0.5 | | `min_curve_velocity` | `double` | Min velocity at lateral acceleration limit [m/ss] | 2.74 | | `decel_distance_before_curve` | `double` | Distance to slowdown before a curve for lateral acceleration limit [m] | 3.5 | @@ -197,12 +198,13 @@ After the optimization, a resampling called `post resampling` is performed befor ### Limit steering angle rate parameters -| Name | Type | Description | Default value | -| :------------------------------- | :------- | :----------------------------------------------------------------------- | :------------ | -| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | -| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | -| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | -| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | +| Name | Type | Description | Default value | +| :------------------------------- | :------- | :------------------------------------------------------------------------------------ | :------------ | +| `enable_steering_rate_limit` | `bool` | To toggle the steer rate filter on and off. You can switch it dynamically at runtime. | true | +| `max_steering_angle_rate` | `double` | Maximum steering angle rate [degree/s] | 40.0 | +| `resample_ds` | `double` | Distance between trajectory points [m] | 0.1 | +| `curvature_threshold` | `double` | If curvature > curvature_threshold, steeringRateLimit is triggered [1/m] | 0.02 | +| `curvature_calculation_distance` | `double` | Distance of points while curvature is calculating [m] | 1.0 | ### Weights for optimization diff --git a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml index bcbf0a5bb6..21ad85137e 100644 --- a/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml +++ b/planning/motion_velocity_smoother/config/default_motion_velocity_smoother.param.yaml @@ -7,12 +7,21 @@ # external velocity limit parameter margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] - # curve parameters + # ---- curve parameters ---- + # - common parameters - + curvature_calculation_distance: 5.0 # distance of points while curvature is calculating for the steer rate and lateral acceleration limit [m] + # - lateral acceleration limit parameters - + enable_lateral_acc_limit: true # To toggle the lateral acc filter on and off. You can switch it dynamically at runtime. max_lateral_accel: 0.8 # max lateral acceleration limit [m/ss] - min_curve_velocity: 2.74 # min velocity at lateral acceleration limit [m/ss] + min_curve_velocity: 2.74 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] + # - steering angle rate limit parameters - + enable_steering_rate_limit: true # To toggle the steer rate filter on and off. You can switch it dynamically at runtime. + max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] + resample_ds: 0.1 # distance between trajectory points [m] + curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] # engage & replan parameters replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] @@ -48,12 +57,6 @@ post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] - # steering angle rate limit parameters - max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] - resample_ds: 0.1 # distance between trajectory points [m] - curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] - curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] - # system over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point 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 503fd4450c..bb49700464 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 @@ -123,6 +123,9 @@ class MotionVelocitySmootherNode : public rclcpp::Node struct Param { + bool enable_lateral_acc_limit; + bool enable_steering_rate_limit; + double max_velocity; // max velocity [m/s] double margin_to_insert_external_velocity_limit; // for external velocity limit [m] double replan_vel_deviation; // if speed error exceeds this [m/s], diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp index 24632c06ba..80f691b5f7 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/resample.hpp @@ -15,14 +15,9 @@ #ifndef MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ #define MOTION_VELOCITY_SMOOTHER__RESAMPLE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include -#include "boost/optional.hpp" - #include namespace motion_velocity_smoother 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 a7416faa3c..beb571635f 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 @@ -15,17 +15,11 @@ #ifndef MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ #define MOTION_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" -#include "motion_velocity_smoother/trajectory_utils.hpp" #include "rclcpp/rclcpp.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "boost/optional.hpp" - #include #include @@ -33,7 +27,6 @@ namespace motion_velocity_smoother { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using vehicle_info_util::VehicleInfoUtil; class SmootherBase { diff --git a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp index 58322fda32..ec8a89c5a3 100644 --- a/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp +++ b/planning/motion_velocity_smoother/include/motion_velocity_smoother/trajectory_utils.hpp @@ -15,9 +15,6 @@ #ifndef MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ #define MOTION_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ -#include "motion_utils/trajectory/trajectory.hpp" -#include "tier4_autoware_utils/geometry/geometry.hpp" - #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "geometry_msgs/msg/pose.hpp" 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 d3c77aa0fa..a4ad118ffe 100644 --- a/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp +++ b/planning/motion_velocity_smoother/src/motion_velocity_smoother_node.cpp @@ -39,7 +39,7 @@ MotionVelocitySmootherNode::MotionVelocitySmootherNode(const rclcpp::NodeOptions using std::placeholders::_1; // set common params - const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; initCommonParam(); over_stop_velocity_warn_thr_ = declare_parameter("over_stop_velocity_warn_thr"); @@ -165,6 +165,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter { auto & p = node_param_; + update_param_bool("enable_lateral_acc_limit", p.enable_lateral_acc_limit); + update_param_bool("enable_steering_rate_limit", p.enable_steering_rate_limit); + update_param("max_velocity", p.max_velocity); update_param( "margin_to_insert_external_velocity_limit", p.margin_to_insert_external_velocity_limit); @@ -266,6 +269,9 @@ rcl_interfaces::msg::SetParametersResult MotionVelocitySmootherNode::onParameter void MotionVelocitySmootherNode::initCommonParam() { auto & p = node_param_; + p.enable_lateral_acc_limit = declare_parameter("enable_lateral_acc_limit"); + p.enable_steering_rate_limit = declare_parameter("enable_steering_rate_limit"); + p.max_velocity = declare_parameter("max_velocity"); // 72.0 kmph p.margin_to_insert_external_velocity_limit = declare_parameter("margin_to_insert_external_velocity_limit"); @@ -572,12 +578,17 @@ bool MotionVelocitySmootherNode::smoothVelocity( // Lateral acceleration limit 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); + const auto traj_lateral_acc_filtered = + node_param_.enable_lateral_acc_limit + ? smoother_->applyLateralAccelerationFilter( + input, initial_motion.vel, initial_motion.acc, enable_smooth_limit, use_resampling) + : input; // 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, false); + node_param_.enable_steering_rate_limit + ? smoother_->applySteeringRateLimit(traj_lateral_acc_filtered, false) + : traj_lateral_acc_filtered; // Resample trajectory with ego-velocity based interval distance auto traj_resampled = smoother_->resampleTrajectory( diff --git a/planning/motion_velocity_smoother/src/resample.cpp b/planning/motion_velocity_smoother/src/resample.cpp index fbf129f105..37f3ce953b 100644 --- a/planning/motion_velocity_smoother/src/resample.cpp +++ b/planning/motion_velocity_smoother/src/resample.cpp @@ -16,6 +16,8 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include 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 a02e432e1e..d1694d7420 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 @@ -16,6 +16,7 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_velocity_smoother/trajectory_utils.hpp" #include #include diff --git a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp index 2e58961d54..4e5efdbfc3 100644 --- a/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/motion_velocity_smoother/src/smoother/smoother_base.cpp @@ -16,8 +16,10 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/tmp_conversion.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "motion_velocity_smoother/resample.hpp" #include "motion_velocity_smoother/trajectory_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include @@ -26,7 +28,6 @@ namespace motion_velocity_smoother { -using vehicle_info_util::VehicleInfoUtil; SmootherBase::SmootherBase(rclcpp::Node & node) { @@ -102,8 +103,6 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( 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. TrajectoryPoints output; const double points_interval = @@ -123,8 +122,8 @@ TrajectoryPoints SmootherBase::applyLateralAccelerationFilter( output = input; } - const size_t idx_dist = - static_cast(std::max(static_cast((curvature_calc_dist) / points_interval), 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); diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index ab5db29ad2..dd361696a5 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -16,6 +16,8 @@ #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" +#include "motion_utils/trajectory/trajectory.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz index 8e62886d25..c3cd89bbce 100644 --- a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz +++ b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz @@ -24,8 +24,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: AutowareDateTimePanel Name: AutowareDateTimePanel - Class: rviz_plugins::AutowareStatePanel @@ -1451,8 +1449,6 @@ Window Geometry: Height: 2032 Hide Left Dock: false Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 RecognitionResultOnImage: collapsed: false diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index b5dd0e91a5..f911fa87be 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -443,7 +443,24 @@ void ObstacleAvoidancePlanner::applyInputVelocity( ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - if (stop_seg_idx) { + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); } } @@ -573,25 +590,25 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, traj_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -599,7 +616,7 @@ std::vector ObstacleAvoidancePlanner::extendTrajectory( full_traj_points, traj_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index f3f1932c44..73bc4a8324 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -106,6 +106,13 @@ successive_num_to_entry_slow_down_condition: 5 successive_num_to_exit_slow_down_condition: 5 + # consider the current ego pose (it is not the nearest pose on the reference trajectory) + # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" + # The both errors decrease with constant rates against the time. + consider_current_pose: + enable_to_consider_current_pose: false + time_to_convergence: 1.5 #[s] + cruise: pid_based_planner: use_velocity_limit_based_planner: true diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 00afc11985..5786a96f72 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -48,6 +48,10 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); // main functions + std::vector createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; std::vector convertToObstacles(const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( @@ -193,6 +197,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double lat_hysteresis_margin_for_slow_down; int successive_num_to_entry_slow_down_condition; int successive_num_to_exit_slow_down_condition; + // consideration for the current ego pose + bool enable_to_consider_current_pose{false}; + double time_to_convergence{1.5}; }; BehaviorDeterminationParam behavior_determination_param_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 5178597bc4..c2b14cc2e8 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -33,6 +33,11 @@ namespace bg = boost::geometry; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin); + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward); @@ -45,9 +50,6 @@ std::vector getCollisionPoints( const double max_lat_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin = 0.0); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 14324709ba..8ef610cc9d 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -131,11 +131,11 @@ double calcObstacleMaxLength(const Shape & shape) } TrajectoryPoint getExtendTrajectoryPoint( - const double extend_distance, const TrajectoryPoint & goal_point) + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) { TrajectoryPoint extend_trajectory_point; - extend_trajectory_point.pose = - tier4_autoware_utils::calcOffsetPose(goal_point.pose, extend_distance, 0.0, 0.0); + extend_trajectory_point.pose = tier4_autoware_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; @@ -147,6 +147,8 @@ std::vector extendTrajectoryPoints( const double step_length) { auto output_points = input_points; + const auto is_driving_forward_opt = motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; if (extend_distance < std::numeric_limits::epsilon()) { return output_points; @@ -156,11 +158,13 @@ std::vector extendTrajectoryPoints( double extend_sum = 0.0; while (extend_sum <= (extend_distance - step_length)) { - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_sum, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); extend_sum += step_length; } - const auto extend_trajectory_point = getExtendTrajectoryPoint(extend_distance, goal_point); + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); output_points.push_back(extend_trajectory_point); return output_points; @@ -270,6 +274,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition"); successive_num_to_exit_slow_down_condition = node.declare_parameter( "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition"); + enable_to_consider_current_pose = node.declare_parameter( + "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); + time_to_convergence = node.declare_parameter( + "behavior_determination.consider_current_pose.time_to_convergence"); } void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( @@ -328,6 +336,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( tier4_autoware_utils::updateParam( parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", successive_num_to_exit_slow_down_condition); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", + enable_to_consider_current_pose); + tier4_autoware_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.time_to_convergence", + time_to_convergence); } ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) @@ -530,6 +544,57 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time); } +std::vector ObstacleCruisePlannerNode::createOneStepPolygons( + const std::vector & traj_points, + const vehicle_info_util::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const +{ + const auto & p = behavior_determination_param_; + const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose; + const double step_length = p.decimate_trajectory_step_length; + const double time_to_convergence = p.time_to_convergence; + + std::vector polygons; + const double current_ego_lat_error = + motion_utils::calcLateralOffset(traj_points, current_ego_pose.position); + const double current_ego_yaw_error = + motion_utils::calcYawDeviation(traj_points, current_ego_pose); + double time_elapsed = 0.0; + + std::vector last_poses = {traj_points.at(0).pose}; + if (is_enable_current_pose_consideration) { + last_poses.push_back(current_ego_pose); + } + + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) { + const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + + current_poses.push_back( + tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += step_length / traj_points.at(i).longitudinal_velocity_mps; + } else { + time_elapsed = std::numeric_limits::max(); + } + } + polygons.push_back( + polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin)); + last_poses = current_poses; + } + return polygons; +} + std::vector ObstacleCruisePlannerNode::convertToObstacles( const std::vector & traj_points) const { @@ -649,7 +714,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( // calculated decimated trajectory points and trajectory polygon const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); const auto decimated_traj_polys = - polygon_utils::createOneStepPolygons(decimated_traj_points, vehicle_info_); + createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -981,8 +1046,8 @@ ObstacleCruisePlannerNode::createCollisionPointForStopObstacle( } // calculate collision points with trajectory with lateral stop margin - const auto traj_polys_with_lat_margin = - polygon_utils::createOneStepPolygons(traj_points, vehicle_info_, p.max_lat_margin_for_stop); + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_); return collision_point; @@ -1043,8 +1108,8 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. - const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons( - traj_points, vehicle_info_, + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 5ea637655a..287df10dce 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -22,6 +22,8 @@ #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" +#include +#include namespace { StopSpeedExceeded createStopSpeedExceededMsg( @@ -378,7 +380,7 @@ std::vector PlannerInterface::generateStopTrajectory( // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, - abs_ego_offset); + abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); @@ -621,7 +623,7 @@ std::vector PlannerInterface::generateSlowDownTrajectory( const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", - planner_data.current_time, i, abs_ego_offset); + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); } @@ -629,14 +631,14 @@ std::vector PlannerInterface::generateSlowDownTrajectory( if (slow_down_start_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", - planner_data.current_time, i * 2, abs_ego_offset); + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } if (slow_down_end_idx) { const auto markers = motion_utils::createSlowDownVirtualWallMarker( slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", - planner_data.current_time, i * 2 + 1, abs_ego_offset); + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray( markers, &debug_data_ptr_->slow_down_debug_wall_marker); } diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index 077b783f0e..e6b4ed89ca 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -31,36 +31,6 @@ geometry_msgs::msg::Point calcOffsetPosition( return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position; } -Polygon2d createOneStepPolygon( - const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - Polygon2d polygon; - - const double base_to_front = vehicle_info.max_longitudinal_offset_m; - const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; - const double base_to_rear = vehicle_info.rear_overhang_m; - - // base step - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, width)); - - // next step - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, -width)); - appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, width)); - - bg::correct(polygon); - - Polygon2d hull_polygon; - bg::convex_hull(polygon, hull_polygon); - - return hull_polygon; -} - PointWithStamp calcNearestCollisionPoint( const size_t first_within_idx, const std::vector & collision_points, const std::vector & decimated_traj_points, const bool is_driving_forward) @@ -132,6 +102,38 @@ std::optional>> getCollisionIndex( namespace polygon_utils { +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) +{ + Polygon2d polygon; + + const double base_to_front = vehicle_info.max_longitudinal_offset_m; + const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin; + const double base_to_rear = vehicle_info.rear_overhang_m; + + for (auto & pose : last_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + for (auto & pose : current_poses) { + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width)); + appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width)); + } + + bg::correct(polygon); + + Polygon2d hull_polygon; + bg::convex_hull(polygon, hull_polygon); + + return hull_polygon; +} + std::optional getCollisionPoint( const std::vector & traj_points, const std::vector & traj_polygons, const Obstacle & obstacle, const bool is_driving_forward) @@ -184,25 +186,4 @@ std::vector getCollisionPoints( return collision_points; } -std::vector createOneStepPolygons( - const std::vector & traj_points, - const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin) -{ - std::vector polygons; - - for (size_t i = 0; i < traj_points.size(); ++i) { - const auto polygon = [&]() { - if (i == 0) { - return createOneStepPolygon( - traj_points.at(i).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - } - return createOneStepPolygon( - traj_points.at(i - 1).pose, traj_points.at(i).pose, vehicle_info, lat_margin); - }(); - - polygons.push_back(polygon); - } - return polygons; -} - } // namespace polygon_utils diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index 8064aa4764..b33342b9de 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -14,6 +14,8 @@ #include "obstacle_stop_planner/adaptive_cruise_control.hpp" +#include "motion_utils/trajectory/trajectory.hpp" + #include #include #include @@ -371,9 +373,7 @@ void AdaptiveCruiseController::calcDistanceToNearestPointOnPath( /* get total distance to collision point */ double dist_to_point = 0; // get distance from self to next nearest point - dist_to_point += boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(trajectory.at(1).pose.position)); + dist_to_point += motion_utils::calcSignedArcLength(trajectory, self_pose.position, size_t(1)); // add distance from next self-nearest-point(=idx:0) to prev point of nearest_point_idx for (int i = 1; i < nearest_point_idx - 1; i++) { @@ -678,15 +678,13 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( const geometry_msgs::msg::Pose self_pose, const double current_vel, const double target_vel, const double dist_to_collision_point, TrajectoryPoints * output_trajectory) { - // plus distance from self to next nearest point - double dist = dist_to_collision_point; + // signed distance from self pose to the point of index 1 double dist_to_first_point = 0.0; + if (output_trajectory->size() > 1) { - dist_to_first_point = boost::geometry::distance( - convertPointRosToBoost(self_pose.position), - convertPointRosToBoost(output_trajectory->at(1).pose.position)); + dist_to_first_point = + motion_utils::calcSignedArcLength(*output_trajectory, self_pose.position, size_t(1)); } - dist += dist_to_first_point; double margin_to_insert = dist_to_collision_point * param_.margin_rate_to_change_vel; // accel = (v_after^2 - v_before^2 ) / 2x @@ -695,7 +693,7 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( const double clipped_acc = boost::algorithm::clamp( target_acc, param_.min_standard_acceleration, param_.max_standard_acceleration); double pre_vel = current_vel; - double total_dist = 0.0; + double total_dist = dist_to_first_point; for (size_t i = 1; i < output_trajectory->size(); i++) { // calc velocity of each point by gradient deceleration const auto current_p = output_trajectory->at(i); @@ -708,7 +706,8 @@ void AdaptiveCruiseController::insertMaxVelocityToPath( next_pre_vel = pre_vel; } else { // v_after = sqrt (2x*accel + v_before^2) - next_pre_vel = std::sqrt(2 * p_dist * clipped_acc + std::pow(pre_vel, 2)); + next_pre_vel = + std::sqrt(2 * std::min(p_dist, total_dist) * clipped_acc + std::pow(pre_vel, 2)); } if (target_acc >= 0) { next_pre_vel = std::min(next_pre_vel, target_vel); diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index 21e45cac79..be0ef13626 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -22,8 +22,10 @@ #include #include +#include #include #include +#include #include diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp index 9ab6093786..123ed997d4 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter_node.cpp @@ -23,7 +23,7 @@ #include "obstacle_velocity_limiter/types.hpp" #include -#include +#include #include #include #include diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index dbd0f2de92..31fe23a47b 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -270,12 +270,36 @@ void ElasticBandSmoother::applyInputVelocity( // insert stop point explicitly const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points); if (stop_idx) { - const auto input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; - const size_t stop_seg_idx = trajectory_utils::findEgoSegmentIndex( - output_traj_points, input_stop_pose, ego_nearest_param_); + const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose; + // NOTE: motion_utils::findNearestSegmentIndex is used instead of + // trajectory_utils::findEgoSegmentIndex + // for the case where input_traj_points is much longer than output_traj_points, and the + // former has a stop point but the latter will not have. + const auto stop_seg_idx = motion_utils::findNearestSegmentIndex( + output_traj_points, input_stop_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); // calculate and insert stop pose on output trajectory - trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, stop_seg_idx); + const bool is_stop_point_inside_trajectory = [&]() { + if (!stop_seg_idx) { + return false; + } + if (*stop_seg_idx == output_traj_points.size() - 2) { + const double signed_projected_length_to_segment = + motion_utils::calcLongitudinalOffsetToSegment( + output_traj_points, *stop_seg_idx, input_stop_pose.position); + const double segment_length = + motion_utils::calcSignedArcLength(output_traj_points, *stop_seg_idx, *stop_seg_idx + 1); + if (segment_length < signed_projected_length_to_segment) { + // NOTE: input_stop_pose is outside output_traj_points. + return false; + } + } + return true; + }(); + if (is_stop_point_inside_trajectory) { + trajectory_utils::insertStopPoint(output_traj_points, input_stop_pose, *stop_seg_idx); + } } time_keeper_ptr_->toc(__func__, " "); @@ -299,25 +323,25 @@ std::vector ElasticBandSmoother::extendTrajectory( const auto joint_end_traj_point_idx = trajectory_utils::getPointIndexAfter( traj_points, joint_start_pose.position, joint_start_traj_seg_idx, joint_traj_max_length_for_smoothing, joint_traj_min_length_for_smoothing); + if (!joint_end_traj_point_idx) { + return trajectory_utils::resampleTrajectoryPoints( + optimized_traj_points, common_param_.output_delta_arc_length); + } // calculate full trajectory points const auto full_traj_points = [&]() { - if (!joint_end_traj_point_idx) { - return optimized_traj_points; - } - - const auto extended_traj_points = std::vector{ + auto extended_traj_points = std::vector{ traj_points.begin() + *joint_end_traj_point_idx, traj_points.end()}; - // NOTE: if optimized_traj_points's back is non zero velocity and extended_traj_points' front is - // zero velocity, the zero velocity will be inserted in the whole joint trajectory. - auto modified_optimized_traj_points = optimized_traj_points; - if (!extended_traj_points.empty() && !modified_optimized_traj_points.empty()) { - modified_optimized_traj_points.back().longitudinal_velocity_mps = - extended_traj_points.front().longitudinal_velocity_mps; + if (!extended_traj_points.empty() && !optimized_traj_points.empty()) { + // NOTE: Without this code, if optimized_traj_points's back is non zero velocity and + // extended_traj_points' front + // is zero velocity, the zero velocity will be inserted in the whole joint trajectory. + // The input stop point will be inserted explicitly in the latter part. + extended_traj_points.front().longitudinal_velocity_mps = + optimized_traj_points.back().longitudinal_velocity_mps; } - - return concatVectors(modified_optimized_traj_points, extended_traj_points); + return concatVectors(optimized_traj_points, extended_traj_points); }(); // resample trajectory points @@ -325,7 +349,7 @@ std::vector ElasticBandSmoother::extendTrajectory( full_traj_points, common_param_.output_delta_arc_length); // update stop velocity on joint - for (size_t i = joint_start_traj_seg_idx + 1; i <= joint_end_traj_point_idx; ++i) { + for (size_t i = joint_start_traj_seg_idx + 1; i <= *joint_end_traj_point_idx; ++i) { if (hasZeroVelocity(traj_points.at(i))) { if (i != 0 && !hasZeroVelocity(traj_points.at(i - 1))) { // Here is when current point is 0 velocity, but previous point is not 0 velocity. diff --git a/planning/path_smoother/src/replan_checker.cpp b/planning/path_smoother/src/replan_checker.cpp index 95c4e1eb1c..11a4a3d5f4 100644 --- a/planning/path_smoother/src/replan_checker.cpp +++ b/planning/path_smoother/src/replan_checker.cpp @@ -14,9 +14,10 @@ #include "path_smoother/replan_checker.hpp" -#include "motion_utils/motion_utils.hpp" +#include "motion_utils/trajectory/trajectory.hpp" #include "path_smoother/utils/trajectory_utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/ros/update_param.hpp" #include diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index a70ab56d98..d460cbf0c1 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -20,7 +20,6 @@ #include "planning_debug_tools/util.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index 1e332e7127..1f0b9a9345 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -18,7 +18,6 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "tier4_autoware_utils/geometry/path_with_lane_id_geometry.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py index 79a97f0033..498f7e458b 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer.py @@ -169,7 +169,7 @@ def publish_recorded_ego_pose(self): 0.06853892326654787, ] - self.recorded_ego_pub.publish(recorded_ego_pose) + self.recorded_ego_pub_as_initialpose.publish(recorded_ego_pose) print("Published recorded ego pose as /initialpose") @@ -182,6 +182,9 @@ def publish_recorded_ego_pose(self): parser.add_argument( "-t", "--tracked-object", help="publish tracked object", action="store_true" ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) args = parser.parse_args() app = QApplication(sys.argv) diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 6b758bfba2..01079fdc3f 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -68,13 +68,20 @@ def __init__(self, args, name): self.pointcloud_pub = self.create_publisher( PointCloud2, "/perception/obstacle_segmentation/pointcloud", 1 ) - self.recorded_ego_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) + self.recorded_ego_pub_as_initialpose = self.create_publisher( + PoseWithCovarianceStamped, "/initialpose", 1 + ) + + self.recorded_ego_pub = self.create_publisher( + Odometry, "/perception_reproducer/rosbag_ego_odom", 1 + ) # load rosbag print("Stared loading rosbag") if os.path.isdir(args.bag): for bag_file in sorted(os.listdir(args.bag)): - self.load_rosbag(args.bag + "/" + bag_file) + if bag_file.endswith(self.args.rosbag_format): + self.load_rosbag(args.bag + "/" + bag_file) else: self.load_rosbag(args.bag) print("Ended loading rosbag") @@ -145,19 +152,26 @@ def kill_online_perception_node(self): except CalledProcessError: pass - def find_topics_by_timestamp(self, timestamp): - objects_data = None - for data in self.rosbag_objects_data: - if timestamp < data[0]: - objects_data = data[1] - break + def binary_search(self, data, timestamp): + low, high = 0, len(data) - 1 - traffic_signals_data = None - for data in self.rosbag_traffic_signals_data: - if timestamp < data[0]: - traffic_signals_data = data[1] - break + while low <= high: + mid = (low + high) // 2 + if data[mid][0] < timestamp: + low = mid + 1 + elif data[mid][0] > timestamp: + high = mid - 1 + else: + return data[mid][1] + # Return the next timestamp's data if available + if low < len(data): + return data[low][1] + return None + + def find_topics_by_timestamp(self, timestamp): + objects_data = self.binary_search(self.rosbag_objects_data, timestamp) + traffic_signals_data = self.binary_search(self.rosbag_traffic_signals_data, timestamp) return objects_data, traffic_signals_data def find_ego_odom_by_timestamp(self, timestamp): diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py index 1b75c86de2..d837abccc0 100755 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_reproducer.py @@ -15,11 +15,12 @@ # limitations under the License. import argparse -import copy +import pickle +import numpy as np from perception_replayer_common import PerceptionReplayerCommon import rclpy -from utils import calc_squared_distance +from utils import StopWatch from utils import create_empty_pointcloud from utils import translate_objects_coordinate @@ -28,18 +29,39 @@ class PerceptionReproducer(PerceptionReplayerCommon): def __init__(self, args): super().__init__(args, "perception_reproducer") - self.ego_pose_idx = None self.prev_traffic_signals_msg = None + self.stopwatch = StopWatch(self.args.verbose) # for debug - # start timer callback + # to make some data to accelerate computation + self.preprocess_data() + + # start main timer callback self.timer = self.create_timer(0.1, self.on_timer) + + # kill perception process to avoid a conflict of the perception topics + self.timer_check_perception_process = self.create_timer(3.0, self.on_timer_kill_perception) + print("Start timer callback") - def on_timer(self): - timestamp = self.get_clock().now().to_msg() + def preprocess_data(self): + # closest search with numpy data is much faster than usual + self.rosbag_ego_odom_data_numpy = np.array( + [ + [data[1].pose.pose.position.x, data[1].pose.pose.position.y] + for data in self.rosbag_ego_odom_data + ] + ) + def on_timer_kill_perception(self): self.kill_online_perception_node() + def on_timer(self): + if self.args.verbose: + print("\n-- on_timer start --") + self.stopwatch.tic("total on_timer") + + timestamp = self.get_clock().now().to_msg() + if self.args.detected_object: pointcloud_msg = create_empty_pointcloud(timestamp) self.pointcloud_pub.publish(pointcloud_msg) @@ -49,15 +71,25 @@ def on_timer(self): return # find nearest ego odom by simulation observation + self.stopwatch.tic("find_nearest_ego_odom_by_observation") ego_odom = self.find_nearest_ego_odom_by_observation(self.ego_pose) pose_timestamp = ego_odom[0] log_ego_pose = ego_odom[1].pose.pose + self.stopwatch.toc("find_nearest_ego_odom_by_observation") # extract message by the nearest ego odom timestamp - msgs = copy.deepcopy(self.find_topics_by_timestamp(pose_timestamp)) + self.stopwatch.tic("find_topics_by_timestamp") + msgs_orig = self.find_topics_by_timestamp(pose_timestamp) + self.stopwatch.toc("find_topics_by_timestamp") + + # copy the messages + self.stopwatch.tic("message deepcopy") + msgs = pickle.loads(pickle.dumps(msgs_orig)) # this is x5 faster than deepcopy objects_msg = msgs[0] traffic_signals_msg = msgs[1] + self.stopwatch.toc("message deepcopy") + self.stopwatch.tic("transform and publish") # objects if objects_msg: objects_msg.header.stamp = timestamp @@ -65,6 +97,9 @@ def on_timer(self): translate_objects_coordinate(self.ego_pose, log_ego_pose, objects_msg) self.objects_pub.publish(objects_msg) + # ego odom + self.recorded_ego_pub.publish(ego_odom[1]) + # traffic signals # temporary support old auto msgs if traffic_signals_msg: @@ -82,23 +117,15 @@ def on_timer(self): else: self.prev_traffic_signals_msg.stamp = timestamp self.traffic_signals_pub.publish(self.prev_traffic_signals_msg) + self.stopwatch.toc("transform and publish") + + self.stopwatch.toc("total on_timer") def find_nearest_ego_odom_by_observation(self, ego_pose): - if self.ego_pose_idx: - start_idx = self.ego_pose_idx - 10 - end_idx = self.ego_pose_idx + 10 - else: - start_idx = 0 - end_idx = len(self.rosbag_ego_odom_data) - 1 - - nearest_idx = 0 - nearest_dist = float("inf") - for idx in range(start_idx, end_idx + 1): - data = self.rosbag_ego_odom_data[idx] - dist = calc_squared_distance(data[1].pose.pose.position, ego_pose.position) - if dist < nearest_dist: - nearest_dist = dist - nearest_idx = idx + # nearest search with numpy format is much (~ x100) faster than regular for loop + self_pose = np.array([ego_pose.position.x, ego_pose.position.y]) + dists_squared = np.sum((self.rosbag_ego_odom_data_numpy - self_pose) ** 2, axis=1) + nearest_idx = np.argmin(dists_squared) return self.rosbag_ego_odom_data[nearest_idx] @@ -112,6 +139,12 @@ def find_nearest_ego_odom_by_observation(self, ego_pose): parser.add_argument( "-t", "--tracked-object", help="publish tracked object", action="store_true" ) + parser.add_argument( + "-f", "--rosbag-format", help="rosbag data format (default is db3)", default="db3" + ) + parser.add_argument( + "-v", "--verbose", help="output debug data", action="store_true", default=False + ) args = parser.parse_args() rclpy.init() diff --git a/planning/planning_debug_tools/scripts/perception_replayer/utils.py b/planning/planning_debug_tools/scripts/perception_replayer/utils.py index 572922d4c7..5ded8d2ea4 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/utils.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/utils.py @@ -15,6 +15,7 @@ # limitations under the License. import math +import time from geometry_msgs.msg import Quaternion import numpy as np @@ -123,3 +124,29 @@ def translate_objects_coordinate(ego_pose, log_ego_pose, objects_msg): object_pose.position.x = object_pos_vec[0] object_pose.position.y = object_pos_vec[1] object_pose.orientation = get_quaternion_from_yaw(log_object_yaw + log_ego_yaw - ego_yaw) + + +class StopWatch: + def __init__(self, verbose): + # A dictionary to store the starting times + self.start_times = {} + self.verbose = verbose + + def tic(self, name): + """Store the current time with the given name.""" + self.start_times[name] = time.perf_counter() + + def toc(self, name): + """Print the elapsed time since the last call to tic() with the same name.""" + if name not in self.start_times: + print(f"No start time found for {name}!") + return + + elapsed_time = ( + time.perf_counter() - self.start_times[name] + ) * 1000 # Convert to milliseconds + if self.verbose: + print(f"Time for {name}: {elapsed_time:.2f} ms") + + # Reset the starting time for the name + del self.start_times[name] diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/planning_validator/utils.hpp index 6c0d2e8694..5831993d74 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/planning_validator/utils.hpp @@ -36,7 +36,9 @@ std::pair getAbsMaxValAndIdx(const std::vector & v); Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval); -void calcCurvature(const Trajectory & trajectory, std::vector & curvatures); +void calcCurvature( + const Trajectory & trajectory, std::vector & curvatures, + const double curvature_distance = 1.0); void calcSteeringAngles( const Trajectory & trajectory, const double wheelbase, std::vector & steering_array); diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 685c282379..1f1c1cc55e 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -5,7 +5,7 @@ 0.1.0 ros node for planning_validator Takamasa Horibe - Yutaka Shimizu + Kosuke Takeuchi Apache License 2.0 Takamasa Horibe diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index 5a90e950bb..3155a6f2e0 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -89,26 +89,68 @@ Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_in return resampled; } -void calcCurvature(const Trajectory & trajectory, std::vector & curvature_arr) +// calculate curvature from three points with curvature_distance +void calcCurvature( + const Trajectory & trajectory, std::vector & curvature_arr, + const double curvature_distance) { if (trajectory.points.size() < 3) { curvature_arr = std::vector(trajectory.points.size(), 0.0); return; } - curvature_arr.push_back(0.0); + // calc arc length array: arc_length(3) - arc_length(0) is distance from point(3) to point(0) + std::vector arc_length(trajectory.points.size(), 0.0); + for (size_t i = 1; i < trajectory.points.size(); ++i) { + arc_length.at(i) = + arc_length.at(i - 1) + calcDistance2d(trajectory.points.at(i - 1), trajectory.points.at(i)); + } + + // initialize with 0 curvature + curvature_arr = std::vector(trajectory.points.size(), 0.0); + + size_t first_distant_index = 0; + size_t last_distant_index = trajectory.points.size() - 1; for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); + // find the previous point + size_t prev_idx = 0; + for (size_t j = i - 1; j > 0; --j) { + if (arc_length.at(i) - arc_length.at(j) > curvature_distance) { + if (first_distant_index == 0) { + first_distant_index = i; // save first index that meets distance requirement + } + prev_idx = j; + break; + } + } + + // find the next point + size_t next_idx = trajectory.points.size() - 1; + for (size_t j = i + 1; j < trajectory.points.size(); ++j) { + if (arc_length.at(j) - arc_length.at(i) > curvature_distance) { + last_distant_index = i; // save last index that meets distance requirement + next_idx = j; + break; + } + } + + const auto p1 = getPoint(trajectory.points.at(prev_idx)); const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + const auto p3 = getPoint(trajectory.points.at(next_idx)); try { - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); + curvature_arr.at(i) = tier4_autoware_utils::calcCurvature(p1, p2, p3); } catch (...) { - // maybe distance is too close - curvature_arr.push_back(0.0); + curvature_arr.at(i) = 0.0; // maybe distance is too close } } - curvature_arr.push_back(0.0); + + // use previous or last curvature where the distance is not enough + for (size_t i = first_distant_index; i > 0; --i) { + curvature_arr.at(i - 1) = curvature_arr.at(i); + } + for (size_t i = last_distant_index; i < curvature_arr.size() - 1; ++i) { + curvature_arr.at(i + 1) = curvature_arr.at(i); + } } std::pair calcMaxCurvature(const Trajectory & trajectory) @@ -117,22 +159,13 @@ std::pair calcMaxCurvature(const Trajectory & trajectory) return {0.0, 0}; } - double max_curvature = 0.0; - size_t max_index = 0; - for (size_t i = 1; i < trajectory.points.size() - 1; ++i) { - const auto p1 = getPoint(trajectory.points.at(i - 1)); - const auto p2 = getPoint(trajectory.points.at(i)); - const auto p3 = getPoint(trajectory.points.at(i + 1)); + std::vector curvature_arr; + calcCurvature(trajectory, curvature_arr); - try { - const auto k = tier4_autoware_utils::calcCurvature(p1, p2, p3); - takeBigger(max_curvature, max_index, std::abs(k), i); - } catch (...) { - // maybe distance is too close - } - } + const auto max_curvature_it = std::max_element(curvature_arr.begin(), curvature_arr.end()); + const size_t index = std::distance(curvature_arr.begin(), max_curvature_it); - return {max_curvature, max_index}; + return {*max_curvature_it, index}; } std::pair calcMaxIntervalDistance(const Trajectory & trajectory) diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 6bd9756098..b12ca7f648 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -15,24 +15,19 @@ #ifndef ROUTE_HANDLER__ROUTE_HANDLER_HPP_ #define ROUTE_HANDLER__ROUTE_HANDLER_HPP_ -#include -#include +#include #include -#include -#include #include -#include #include #include #include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -41,8 +36,6 @@ namespace route_handler { using autoware_auto_mapping_msgs::msg::HADMapBin; -using autoware_auto_planning_msgs::msg::Path; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::LaneletSegment; diff --git a/planning/route_handler/package.xml b/planning/route_handler/package.xml index 6304e21932..29f0411a5a 100644 --- a/planning/route_handler/package.xml +++ b/planning/route_handler/package.xml @@ -9,6 +9,7 @@ Yutaka Shimizu Kosuke Takeuchi Takayuki Murooka + Mamoru Sobue Apache License 2.0 Fumiya Watanabe diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 3d9dd8fe7a..ba9c46f157 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -21,9 +21,15 @@ #include #include -#include +#include +#include +#include + #include #include +#include +#include +#include #include #include @@ -36,6 +42,7 @@ namespace { using autoware_auto_planning_msgs::msg::Path; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_planning_msgs::msg::LaneletPrimitive; using geometry_msgs::msg::Pose; diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp index 91b27c74a7..8c8c346557 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp @@ -18,6 +18,8 @@ #include "route_handler/route_handler.hpp" #include "static_centerline_optimizer/type_alias.hpp" +#include + #include #include #include diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz index c2df606fba..1e7573d561 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz +++ b/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz @@ -18,8 +18,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - Class: rviz_common/Time Experimental: false Name: Time @@ -423,8 +421,6 @@ Window Geometry: Height: 1043 Hide Left Dock: false Hide Right Dock: false - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 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 Selection: collapsed: false diff --git a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp index 8c3b84854a..4532eb6697 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp +++ b/planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp @@ -32,8 +32,10 @@ #include #include +#include #include +#include #include #include diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index 7871eb9e95..d5169ac17a 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -18,6 +18,9 @@ #include "behavior_path_planner/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/marker_helper.hpp" + +#include +#include namespace static_centerline_optimizer { namespace diff --git a/planning/surround_obstacle_checker/CMakeLists.txt b/planning/surround_obstacle_checker/CMakeLists.txt index 0d7b636646..694bddf421 100644 --- a/planning/surround_obstacle_checker/CMakeLists.txt +++ b/planning/surround_obstacle_checker/CMakeLists.txt @@ -18,6 +18,12 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/node.cpp ) +if(tf2_geometry_msgs_VERSION VERSION_LESS 0.25.4) + target_compile_definitions(${PROJECT_NAME} PRIVATE + DEFINE_LEGACY_FUNCTION + ) +endif() + target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES} ) diff --git a/planning/surround_obstacle_checker/src/debug_marker.cpp b/planning/surround_obstacle_checker/src/debug_marker.cpp index d3d73a89e0..ebe5552c44 100644 --- a/planning/surround_obstacle_checker/src/debug_marker.cpp +++ b/planning/surround_obstacle_checker/src/debug_marker.cpp @@ -14,6 +14,7 @@ #include "surround_obstacle_checker/debug_marker.hpp" +#include #include #include #include diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml index 7d80bc903a..5bd7502098 100644 --- a/sensing/gnss_poser/package.xml +++ b/sensing/gnss_poser/package.xml @@ -5,6 +5,7 @@ 1.0.0 The ROS 2 gnss_poser package Yamato Ando + Koji Minoda Apache License 2.0 Ryo Watanabe diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 7466d158da..3dff79bbf9 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -13,6 +13,7 @@ ament_cmake_auto autoware_cmake + diagnostic_updater rclcpp rclcpp_components sensor_msgs diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index 581eccb009..5f41a9089b 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -83,6 +83,14 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); + // Validation const bool is_bias_small_enough = std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && diff --git a/sensing/pointcloud_preprocessor/docs/concatenate-data.md b/sensing/pointcloud_preprocessor/docs/concatenate-data.md index a3b5268815..74b4f3df17 100644 --- a/sensing/pointcloud_preprocessor/docs/concatenate-data.md +++ b/sensing/pointcloud_preprocessor/docs/concatenate-data.md @@ -42,6 +42,7 @@ The figure below represents the reception time of each sensor data and how it is | `timeout_sec` | double | 0.1 | tolerance of time to publish next pointcloud [s]
When this time limit is exceeded, the filter concatenates and publishes pointcloud, even if not all the point clouds are subscribed. | | `input_offset` | vector of double | [] | This parameter can control waiting time for each input sensor pointcloud [s]. You must to set the same length of offsets with input pointclouds numbers.
For its tuning, please see [actual usage page](#how-to-tuning-timeout_sec-and-input_offset). | | `publish_synchronized_pointcloud` | bool | false | If true, publish the time synchronized pointclouds. All input pointclouds are transformed and then re-published as message named `_synchronized`. | +| `input_twist_topic_type` | std::string | twist | Topic type for twist. Currently support `twist` or `odom`. | ## Actual Usage diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp index f5d55a2ebc..be96aa9438 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_nodelet.hpp @@ -71,6 +71,7 @@ #include #include #include +#include #include #include #include @@ -88,6 +89,7 @@ namespace pointcloud_preprocessor { using autoware_point_types::PointXYZI; using point_cloud_msg_wrapper::PointCloud2Modifier; + /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -131,10 +133,13 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node std::vector::SharedPtr> filters_; rclcpp::Subscription::SharedPtr sub_twist_; + rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::TimerBase::SharedPtr timer_; diagnostic_updater::Updater updater_{this}; + const std::string input_twist_topic_type_; + /** \brief Output TF frame the concatenated points should be transformed to. */ std::string output_frame_; @@ -170,6 +175,7 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr input); + void odom_callback(const nav_msgs::msg::Odometry::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py index 6b1fc0f5bd..0d65b45b1b 100644 --- a/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py +++ b/sensing/pointcloud_preprocessor/launch/preprocessor.launch.py @@ -38,13 +38,17 @@ def launch_setup(context, *args, **kwargs): package=pkg, plugin="pointcloud_preprocessor::PointCloudConcatenateDataSynchronizerComponent", name="sync_and_concatenate_filter", - remappings=[("output", "points_raw/concatenated")], + remappings=[ + ("~/input/twist", "/sensing/vehicle_velocity_converter/twist_with_covariance"), + ("output", "points_raw/concatenated"), + ], parameters=[ { "input_topics": LaunchConfiguration("input_points_raw_list"), "output_frame": LaunchConfiguration("tf_output_frame"), "approximate_sync": True, "publish_synchronized_pointcloud": False, + "input_twist_topic_type": "twist", } ], ) diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp index d88665a4c4..ff72e433e9 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_and_time_sync_nodelet.cpp @@ -69,7 +69,8 @@ namespace pointcloud_preprocessor { PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchronizerComponent( const rclcpp::NodeOptions & node_options) -: Node("point_cloud_concatenator_component", node_options) +: Node("point_cloud_concatenator_component", node_options), + input_twist_topic_type_(declare_parameter("input_twist_topic_type", "twist")) { // initialize debug tool { @@ -164,10 +165,24 @@ PointCloudConcatenateDataSynchronizerComponent::PointCloudConcatenateDataSynchro filters_[d] = this->create_subscription( input_topics_[d], rclcpp::SensorDataQoS().keep_last(maximum_queue_size_), cb); } - auto twist_cb = std::bind( - &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, std::placeholders::_1); - sub_twist_ = this->create_subscription( - "~/input/twist", rclcpp::QoS{100}, twist_cb); + + if (input_twist_topic_type_ == "twist") { + auto twist_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::twist_callback, this, + std::placeholders::_1); + sub_twist_ = this->create_subscription( + "~/input/twist", rclcpp::QoS{100}, twist_cb); + } else if (input_twist_topic_type_ == "odom") { + auto odom_cb = std::bind( + &PointCloudConcatenateDataSynchronizerComponent::odom_callback, this, + std::placeholders::_1); + sub_odom_ = this->create_subscription( + "~/input/odom", rclcpp::QoS{100}, odom_cb); + } else { + RCLCPP_ERROR_STREAM( + get_logger(), "input_twist_topic_type is invalid: " << input_twist_topic_type_); + throw std::runtime_error("input_twist_topic_type is invalid: " + input_twist_topic_type_); + } } // Transformed Raw PointCloud2 Publisher @@ -227,8 +242,19 @@ Eigen::Matrix4f PointCloudConcatenateDataSynchronizerComponent::computeTransformToAdjustForOldTimestamp( const rclcpp::Time & old_stamp, const rclcpp::Time & new_stamp) { - // return identity if no twist is available or old_stamp is newer than new_stamp - if (twist_ptr_queue_.empty() || old_stamp > new_stamp) { + // return identity if no twist is available + if (twist_ptr_queue_.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "No twist is available. Please confirm twist topic and timestamp"); + return Eigen::Matrix4f::Identity(); + } + + // return identity if old_stamp is newer than new_stamp + if (old_stamp > new_stamp) { + RCLCPP_WARN_STREAM_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(10000).count(), + "old_stamp is newer than new_stamp,"); return Eigen::Matrix4f::Identity(); } @@ -400,12 +426,6 @@ void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( { output_ptr->header = input_ptr->header; - if (input_ptr->data.empty()) { - RCLCPP_WARN_STREAM_THROTTLE( - this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - return; - } - PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; output_modifier.reserve(input_ptr->width); @@ -460,6 +480,12 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( { std::lock_guard lock(mutex_); auto input = std::make_shared(*input_ptr); + if (input->data.empty()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); + return; + } + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); convertToXYZICloud(input, xyzi_input_ptr); @@ -558,6 +584,32 @@ void PointCloudConcatenateDataSynchronizerComponent::twist_callback( twist_ptr_queue_.push_back(twist_ptr); } +void PointCloudConcatenateDataSynchronizerComponent::odom_callback( + const nav_msgs::msg::Odometry::ConstSharedPtr input) +{ + // if rosbag restart, clear buffer + if (!twist_ptr_queue_.empty()) { + if (rclcpp::Time(twist_ptr_queue_.front()->header.stamp) > rclcpp::Time(input->header.stamp)) { + twist_ptr_queue_.clear(); + } + } + + // pop old data + while (!twist_ptr_queue_.empty()) { + if ( + rclcpp::Time(twist_ptr_queue_.front()->header.stamp) + rclcpp::Duration::from_seconds(1.0) > + rclcpp::Time(input->header.stamp)) { + break; + } + twist_ptr_queue_.pop_front(); + } + + auto twist_ptr = std::make_shared(); + twist_ptr->header = input->header; + twist_ptr->twist = input->twist.twist; + twist_ptr_queue_.push_back(twist_ptr); +} + void PointCloudConcatenateDataSynchronizerComponent::checkConcatStatus( diagnostic_updater::DiagnosticStatusWrapper & stat) { 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 8eca49d9ff..4b25dba8b1 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 @@ -269,7 +269,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node * @brief ControlModeRequest server */ void on_control_mode_request( - const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Request::ConstSharedPtr request, const ControlModeCommand::Response::SharedPtr response); /** 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 fe28d063ee..8c2688928b 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 @@ -117,16 +117,18 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt "input/initialtwist", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialtwist, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_ackermann_cmd_ = *msg; }); + [this](const AckermannControlCommand::ConstSharedPtr msg) { current_ackermann_cmd_ = *msg; }); sub_manual_ackermann_cmd_ = create_subscription( "input/manual_ackermann_control_command", QoS{1}, - [this](const AckermannControlCommand::SharedPtr msg) { current_manual_ackermann_cmd_ = *msg; }); + [this](const AckermannControlCommand::ConstSharedPtr msg) { + current_manual_ackermann_cmd_ = *msg; + }); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, - [this](const GearCommand::SharedPtr msg) { current_gear_cmd_ = *msg; }); + [this](const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ = *msg; }); sub_manual_gear_cmd_ = create_subscription( "input/manual_gear_command", QoS{1}, - [this](const GearCommand::SharedPtr msg) { current_manual_gear_cmd_ = *msg; }); + [this](const GearCommand::ConstSharedPtr msg) { current_manual_gear_cmd_ = *msg; }); sub_turn_indicators_cmd_ = create_subscription( "input/turn_indicators_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1)); @@ -484,7 +486,7 @@ void SimplePlanningSimulator::on_engage(const Engage::ConstSharedPtr msg) } void SimplePlanningSimulator::on_control_mode_request( - const ControlModeCommand::Request::SharedPtr request, + const ControlModeCommand::Request::ConstSharedPtr request, const ControlModeCommand::Response::SharedPtr response) { const auto m = request->mode; diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index e57a3683b9..cedfec3951 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -49,7 +49,7 @@ class PubSubNode : public rclcpp::Node { current_odom_sub_ = create_subscription( "output/odometry", rclcpp::QoS{1}, - [this](const Odometry::SharedPtr msg) { current_odom_ = msg; }); + [this](const Odometry::ConstSharedPtr msg) { current_odom_ = msg; }); pub_ackermann_command_ = create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); pub_initialpose_ = @@ -62,7 +62,7 @@ class PubSubNode : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_gear_cmd_; rclcpp::Publisher::SharedPtr pub_initialpose_; - Odometry::SharedPtr current_odom_; + Odometry::ConstSharedPtr current_odom_; }; /** diff --git a/system/autoware_auto_msgs_adapter/CMakeLists.txt b/system/autoware_auto_msgs_adapter/CMakeLists.txt index 9b48112227..ff8dcf6ced 100644 --- a/system/autoware_auto_msgs_adapter/CMakeLists.txt +++ b/system/autoware_auto_msgs_adapter/CMakeLists.txt @@ -12,6 +12,7 @@ ament_auto_add_library(${NODE_NAME} src/include/adapter_base.hpp src/include/adapter_base_interface.hpp src/include/adapter_control.hpp + src/include/adapter_perception.hpp src/include/autoware_auto_msgs_adapter_core.hpp src/autoware_auto_msgs_adapter_core.cpp) diff --git a/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml b/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml new file mode 100644 index 0000000000..d8b6e31543 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/config/adapter_perception.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + msg_type_target: "autoware_auto_perception_msgs/msg/PredictedObjects" + topic_name_source: "/perception/object_recognition/objects" + topic_name_target: "/perception/object_recognition/objects_auto" diff --git a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml index 89b58e2d60..f914fe6e82 100755 --- a/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml +++ b/system/autoware_auto_msgs_adapter/launch/autoware_auto_msgs_adapter.launch.xml @@ -1,8 +1,12 @@ - + + - - + + + + + diff --git a/system/autoware_auto_msgs_adapter/package.xml b/system/autoware_auto_msgs_adapter/package.xml index 0deba020ad..de19032e1a 100644 --- a/system/autoware_auto_msgs_adapter/package.xml +++ b/system/autoware_auto_msgs_adapter/package.xml @@ -22,7 +22,9 @@ autoware_lint_common autoware_auto_control_msgs + autoware_auto_perception_msgs autoware_control_msgs + autoware_perception_msgs rclcpp rclcpp_components diff --git a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json index cf853ee1da..35e3aef151 100644 --- a/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json +++ b/system/autoware_auto_msgs_adapter/schema/autoware_auto_msgs_adapter.schema.json @@ -9,7 +9,10 @@ "msg_type_target": { "type": "string", "description": "Target message type", - "enum": ["autoware_auto_control_msgs/msg/AckermannControlCommand"], + "enum": [ + "autoware_auto_control_msgs/msg/AckermannControlCommand", + "autoware_auto_perception_msgs/msg/PredictedObjects" + ], "default": "autoware_auto_control_msgs/msg/AckermannControlCommand" }, "topic_name_source": { diff --git a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp index 98fe916903..0f65571aba 100644 --- a/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp +++ b/system/autoware_auto_msgs_adapter/src/autoware_auto_msgs_adapter_core.cpp @@ -57,6 +57,11 @@ MapStringAdapter AutowareAutoMsgsAdapterNode::create_adapter_map( return std::static_pointer_cast( std::make_shared(*this, topic_name_source, topic_name_target)); }}, + {"autoware_auto_perception_msgs/msg/PredictedObjects", + [&] { + return std::static_pointer_cast( + std::make_shared(*this, topic_name_source, topic_name_target)); + }}, }; } diff --git a/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp new file mode 100644 index 0000000000..3821bbad8c --- /dev/null +++ b/system/autoware_auto_msgs_adapter/src/include/adapter_perception.hpp @@ -0,0 +1,97 @@ +// Copyright 2023 The 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 ADAPTER_PERCEPTION_HPP_ +#define ADAPTER_PERCEPTION_HPP_ + +#include "adapter_base.hpp" + +#include + +#include +#include + +#include + +namespace autoware_auto_msgs_adapter +{ +using Objects_auto = autoware_auto_perception_msgs::msg::PredictedObjects; +using Objects = autoware_perception_msgs::msg::PredictedObjects; + +class AdapterPerception : public autoware_auto_msgs_adapter::AdapterBase +{ +public: + AdapterPerception( + rclcpp::Node & node, const std::string & topic_name_source, + const std::string & topic_name_target, const rclcpp::QoS & qos = rclcpp::QoS{1}) + : AdapterBase(node, topic_name_source, topic_name_target, qos) + { + RCLCPP_DEBUG( + node.get_logger(), "AdapterPerception is initialized to convert: %s -> %s", + topic_name_source.c_str(), topic_name_target.c_str()); + } + +protected: + Objects_auto convert(const Objects & msg_source) override + { + Objects_auto msg_auto; + msg_auto.header = msg_source.header; + + autoware_auto_perception_msgs::msg::PredictedObject object_auto; + for (size_t it_of_objects = 0; it_of_objects < msg_source.objects.size(); it_of_objects++) { + // convert id and probability + object_auto.object_id = msg_source.objects[it_of_objects].object_id; + object_auto.existence_probability = msg_source.objects[it_of_objects].existence_probability; + // convert classification + autoware_auto_perception_msgs::msg::ObjectClassification classification; + for (size_t i = 0; i < msg_source.objects[it_of_objects].classification.size(); i++) { + classification.label = msg_source.objects[it_of_objects].classification[i].label; + classification.probability = + msg_source.objects[it_of_objects].classification[i].probability; + object_auto.classification.push_back(classification); + } + // convert kinematics + object_auto.kinematics.initial_pose_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_pose_with_covariance; + object_auto.kinematics.initial_twist_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_twist_with_covariance; + object_auto.kinematics.initial_acceleration_with_covariance = + msg_source.objects[it_of_objects].kinematics.initial_acceleration_with_covariance; + for (size_t j = 0; j < msg_source.objects[it_of_objects].kinematics.predicted_paths.size(); + j++) { + autoware_auto_perception_msgs::msg::PredictedPath predicted_path; + for (size_t k = 0; + k < msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path.size(); k++) { + predicted_path.path.push_back( + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].path[k]); + } + predicted_path.time_step = + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].time_step; + predicted_path.confidence = + msg_source.objects[it_of_objects].kinematics.predicted_paths[j].confidence; + object_auto.kinematics.predicted_paths.push_back(predicted_path); + } + // convert shape + object_auto.shape.type = msg_source.objects[it_of_objects].shape.type; + object_auto.shape.footprint = msg_source.objects[it_of_objects].shape.footprint; + object_auto.shape.dimensions = msg_source.objects[it_of_objects].shape.dimensions; + + // add to objects list + msg_auto.objects.push_back(object_auto); + } + return msg_auto; + } +}; +} // namespace autoware_auto_msgs_adapter + +#endif // ADAPTER_PERCEPTION_HPP_ diff --git a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp index a33a88c686..f595359dcc 100644 --- a/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp +++ b/system/autoware_auto_msgs_adapter/src/include/autoware_auto_msgs_adapter_core.hpp @@ -15,6 +15,7 @@ #define AUTOWARE_AUTO_MSGS_ADAPTER_CORE_HPP_ #include "adapter_control.hpp" +#include "adapter_perception.hpp" #include diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp index e33ae07e6a..430b9a26b6 100644 --- a/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp +++ b/system/autoware_auto_msgs_adapter/test/test_msg_ackermann_control_command.cpp @@ -112,5 +112,5 @@ TEST(AutowareAutoMsgsAdapter, TestMsgAckermannControlCommand) // NOLINT for gte EXPECT_TRUE(test_completed); EXPECT_FALSE(timed_out); - rclcpp::shutdown(); + // rclcpp::shutdown(); } diff --git a/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp new file mode 100644 index 0000000000..9f7f0ce805 --- /dev/null +++ b/system/autoware_auto_msgs_adapter/test/test_msg_predicted_objects.cpp @@ -0,0 +1,267 @@ +// Copyright 2023 The 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 + +#include + +#include + +autoware_perception_msgs::msg::PredictedObjects generate_perception_msg() +{ + // generate deterministic random int + std::mt19937 gen(0); + std::uniform_int_distribution<> dis_int(0, 1000000); + auto rand_int = [&dis_int, &gen]() { return dis_int(gen); }; + + autoware_perception_msgs::msg::PredictedObjects msg_perception; + msg_perception.header.stamp = rclcpp::Time(rand_int()); + msg_perception.header.frame_id = "test_frame"; + + autoware_perception_msgs::msg::PredictedObject obj; + // // { + unique_identifier_msgs::msg::UUID uuid_; + std::independent_bits_engine bit_eng(gen); + std::generate(uuid_.uuid.begin(), uuid_.uuid.end(), bit_eng); + obj.object_id = uuid_; + obj.existence_probability = 0.5; + autoware_perception_msgs::msg::ObjectClassification obj_class; + obj_class.label = 0; + obj_class.probability = 0.5; + obj.classification.push_back(obj_class); + + // { + autoware_perception_msgs::msg::PredictedObjectKinematics kin; + kin.initial_pose_with_covariance.pose.position.x = 10; + kin.initial_pose_with_covariance.pose.position.y = 10; + kin.initial_pose_with_covariance.pose.position.z = 0; + kin.initial_pose_with_covariance.pose.orientation.x = 0; + kin.initial_pose_with_covariance.pose.orientation.y = 0; + kin.initial_pose_with_covariance.pose.orientation.z = 0; + kin.initial_pose_with_covariance.pose.orientation.w = 1; + + kin.initial_twist_with_covariance.twist.linear.x = 1; + kin.initial_twist_with_covariance.twist.linear.y = 0; + kin.initial_twist_with_covariance.twist.linear.z = 0; + kin.initial_twist_with_covariance.twist.angular.x = 0; + kin.initial_twist_with_covariance.twist.angular.y = 0; + kin.initial_twist_with_covariance.twist.angular.z = 0; + + kin.initial_acceleration_with_covariance.accel.linear.x = 0; + kin.initial_acceleration_with_covariance.accel.linear.y = 0; + kin.initial_acceleration_with_covariance.accel.linear.z = 0; + kin.initial_acceleration_with_covariance.accel.angular.x = 0; + kin.initial_acceleration_with_covariance.accel.angular.y = 0; + kin.initial_acceleration_with_covariance.accel.angular.z = 0; + + for (size_t i = 0; i < 10; i++) { + kin.predicted_paths[0].path[i].position.x = i; + kin.predicted_paths[0].path[i].position.y = 0; + kin.predicted_paths[0].path[i].position.z = 0; + } + obj.kinematics = kin; + // } + // { + autoware_perception_msgs::msg::Shape s; + s.type = 1; + geometry_msgs::msg::Point32 p; + p.x = 9.0f; + p.y = 11.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 11.0f; + p.y = 11.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 11.0f; + p.y = 9.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + p.x = 9.0f; + p.y = 9.0f; + p.z = 0.0f; + s.footprint.points.push_back(p); + + s.dimensions.x = 2; + s.dimensions.y = 2; + s.dimensions.z = 2; + + obj.shape = s; + // } + // } + + msg_perception.objects.push_back(obj); + return msg_perception; +} + +TEST(AutowareAutoMsgsAdapter, TestPredictedObjects) // NOLINT for gtest +{ + const std::string msg_type_target = "autoware_auto_perception_msgs/msg/PredictedObjects"; + const std::string topic_name_source = "topic_name_source"; + const std::string topic_name_target = "topic_name_target"; + + std::cout << "Creating the adapter node..." << std::endl; + + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("msg_type_target", msg_type_target); + node_options.append_parameter_override("topic_name_source", topic_name_source); + node_options.append_parameter_override("topic_name_target", topic_name_target); + + using autoware_auto_msgs_adapter::AutowareAutoMsgsAdapterNode; + AutowareAutoMsgsAdapterNode::SharedPtr node_adapter; + node_adapter = std::make_shared(node_options); + + std::cout << "Creating the subscriber node..." << std::endl; + + auto node_subscriber = std::make_shared("node_subscriber", rclcpp::NodeOptions{}); + + bool test_completed = false; + + const auto msg_perception = generate_perception_msg(); + auto sub = node_subscriber->create_subscription< + autoware_auto_perception_msgs::msg::PredictedObjects>( + topic_name_target, rclcpp::QoS{1}, + [&msg_perception, + &test_completed](const autoware_auto_perception_msgs::msg::PredictedObjects::SharedPtr msg) { + EXPECT_EQ(msg->header.stamp, msg_perception.header.stamp); + EXPECT_EQ(msg->header.frame_id, msg_perception.header.frame_id); + EXPECT_EQ(msg->objects[0].object_id.uuid, msg_perception.objects[0].object_id.uuid); + EXPECT_FLOAT_EQ( + msg->objects[0].existence_probability, msg_perception.objects[0].existence_probability); + EXPECT_EQ( + msg->objects[0].classification[0].label, msg_perception.objects[0].classification[0].label); + EXPECT_FLOAT_EQ( + msg->objects[0].classification[0].probability, + msg_perception.objects[0].classification[0].probability); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.x, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.y, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.position.z, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.position.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w, + msg_perception.objects[0].kinematics.initial_pose_with_covariance.pose.orientation.w); + + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.x, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.y, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.linear.z, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.linear.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.x, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.y, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_twist_with_covariance.twist.angular.z, + msg_perception.objects[0].kinematics.initial_twist_with_covariance.twist.angular.z); + + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.linear.z); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z, + msg_perception.objects[0].kinematics.initial_acceleration_with_covariance.accel.angular.z); + + for (size_t i = 0; i < msg->objects[0].kinematics.predicted_paths[0].path.size(); i++) { + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.x, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.x); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.y, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.y); + EXPECT_FLOAT_EQ( + msg->objects[0].kinematics.predicted_paths[0].path[i].position.z, + msg_perception.objects[0].kinematics.predicted_paths[0].path[i].position.z); + } + + EXPECT_EQ(msg->objects[0].shape.type, msg_perception.objects[0].shape.type); + for (size_t i = 0; i < msg_perception.objects[0].shape.footprint.points.size(); i++) { + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].x, + msg_perception.objects[0].shape.footprint.points[i].x); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].y, + msg_perception.objects[0].shape.footprint.points[i].y); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.footprint.points[i].z, + msg_perception.objects[0].shape.footprint.points[i].z); + } + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.x, msg_perception.objects[0].shape.dimensions.x); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.y, msg_perception.objects[0].shape.dimensions.y); + EXPECT_FLOAT_EQ( + msg->objects[0].shape.dimensions.z, msg_perception.objects[0].shape.dimensions.z); + + test_completed = true; + }); + + std::cout << "Creating the publisher node..." << std::endl; + + auto node_publisher = std::make_shared("node_publisher", rclcpp::NodeOptions{}); + auto pub = node_publisher->create_publisher( + topic_name_source, rclcpp::QoS{1}); + pub->publish(msg_perception); + + auto start_time = std::chrono::system_clock::now(); + auto max_test_dur = std::chrono::seconds(5); + auto timed_out = false; + + while (rclcpp::ok() && !test_completed) { + rclcpp::spin_some(node_subscriber); + rclcpp::spin_some(node_adapter); + rclcpp::spin_some(node_publisher); + rclcpp::sleep_for(std::chrono::milliseconds(50)); + if (std::chrono::system_clock::now() - start_time > max_test_dur) { + timed_out = true; + break; + } + } + + EXPECT_TRUE(test_completed); + EXPECT_FALSE(timed_out); + + // rclcpp::shutdown(); +} diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt new file mode 100644 index 0000000000..54aebe6f37 --- /dev/null +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.14) +project(system_diagnostic_graph) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_executable(aggregator + src/main.cpp + src/core/config.cpp + src/core/debug.cpp + src/core/graph.cpp + src/core/node.cpp + src/core/expr.cpp + src/core/update.cpp +) + +ament_auto_add_executable(converter + src/tool.cpp +) + +install( + PROGRAMS util/debug.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/README.md b/system/system_diagnostic_graph/README.md new file mode 100644 index 0000000000..c2c9993834 --- /dev/null +++ b/system/system_diagnostic_graph/README.md @@ -0,0 +1,45 @@ +# System diagnostic graph + +## Overview + +The system diagnostic graph node subscribes to diagnostic status and publishes aggregated diagnostic status. +As shown in the diagram below, this node introduces extra diagnostic status for intermediate functional unit. +Diagnostic status dependencies will be directed acyclic graph (DAG). + +![overview](./doc/overview.drawio.svg) + +## Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | --------------------------- | --------------------------------------- | ------------------ | +| subscription | `/diagnostics` | `diagnostic_msgs/msg/DiagnosticArray` | Input diagnostics. | +| publisher | `/diagnostics_graph/status` | `diagnostic_msgs/msg/DiagnosticArray` | Graph status. | +| publisher | `/diagnostics_graph/struct` | `tier4_system_msgs/msg/DiagnosticGraph` | Graph structure. | + +## Parameters + +| Parameter Name | Data Type | Description | +| ------------------ | --------- | ------------------------------------------ | +| `graph_file` | `string` | Path of the config file. | +| `rate` | `double` | Rate of aggregation and topic publication. | +| `status_qos_depth` | `uint` | QoS depth of status topic. | +| `source_qos_depth` | `uint` | QoS depth of source topic. | + +## Graph file format + +- [GraphFile](./doc/format/graph-file.md) +- [Path](./doc/format/path.md) +- [Node](./doc/format/node.md) + - [Diag](./doc/format/diag.md) + - [Unit](./doc/format/unit.md) + - [And](./doc/format/and.md) + - [Or](./doc/format/or.md) + +## Example + +- [example1.yaml](./example/example1.yaml) +- [example2.yaml](./example/example2.yaml) + +```bash +ros2 launch system_diagnostic_graph example.launch.xml +``` diff --git a/system/system_diagnostic_graph/config/default.param.yaml b/system/system_diagnostic_graph/config/default.param.yaml new file mode 100644 index 0000000000..f02247374b --- /dev/null +++ b/system/system_diagnostic_graph/config/default.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + rate: 1.0 + input_qos_depth: 1000 + graph_qos_depth: 1 diff --git a/system/system_diagnostic_graph/doc/format/and.md b/system/system_diagnostic_graph/doc/format/and.md new file mode 100644 index 0000000000..a92aa28178 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/and.md @@ -0,0 +1,11 @@ +# Unit + +And is a node that is evaluated as the AND of the input nodes. + +## Format + +| Name | Type | Required | Description | +| ---- | ------------------------------------------ | -------- | ------------------------------------- | +| type | string | yes | Specify `and` when using this object. | +| name | string | yes | Name of diagnostic status. | +| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/system_diagnostic_graph/doc/format/diag.md b/system/system_diagnostic_graph/doc/format/diag.md new file mode 100644 index 0000000000..9835924f08 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/diag.md @@ -0,0 +1,11 @@ +# Diag + +Diag is a node that refers to a source diagnostics. + +## Format + +| Name | Type | Required | Description | +| -------- | ------ | -------- | -------------------------------------- | +| type | string | yes | Specify `diag` when using this object. | +| name | string | yes | Name of diagnostic status. | +| hardware | string | yes | Hardware ID of diagnostic status. | diff --git a/system/system_diagnostic_graph/doc/format/graph-file.md b/system/system_diagnostic_graph/doc/format/graph-file.md new file mode 100644 index 0000000000..3c4cfec996 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/graph-file.md @@ -0,0 +1,10 @@ +# GraphFile + +GraphFile is the top level object that makes up the configuration file. + +## Format + +| Name | Type | Required | Description | +| ----- | ----------------------- | -------- | ------------------------------ | +| files | List<[Path](./path.md)> | no | Paths of the files to include. | +| nodes | List<[Node](./node.md)> | no | Nodes of the diagnostic graph. | diff --git a/system/system_diagnostic_graph/doc/format/node.md b/system/system_diagnostic_graph/doc/format/node.md new file mode 100644 index 0000000000..da8e8e57b1 --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/node.md @@ -0,0 +1,9 @@ +# Node + +Node is a base object that makes up the diagnostic graph. + +## Format + +| Name | Type | Required | Description | +| ---- | ------ | -------- | ------------------------------------------- | +| type | string | yes | Node type. See derived objects for details. | diff --git a/system/system_diagnostic_graph/doc/format/or.md b/system/system_diagnostic_graph/doc/format/or.md new file mode 100644 index 0000000000..3e668b686c --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/or.md @@ -0,0 +1,11 @@ +# Unit + +Or is a node that is evaluated as the OR of the input nodes. + +## Format + +| Name | Type | Required | Description | +| ---- | ------------------------------------------ | -------- | ------------------------------------ | +| type | string | yes | Specify `or` when using this object. | +| name | string | yes | Name of diagnostic status. | +| list | List<[Diag](./diag.md)\|[Unit](./unit.md)> | yes | List of input node references. | diff --git a/system/system_diagnostic_graph/doc/format/path.md b/system/system_diagnostic_graph/doc/format/path.md new file mode 100644 index 0000000000..1f27accefa --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/path.md @@ -0,0 +1,10 @@ +# Path + +Path is an object that indicates the path of the file to include. + +## Format + +| Name | Type | Required | Description | +| ------- | ------ | -------- | ----------------------------- | +| package | string | yes | Package name. | +| path | string | yes | Relative path in the package. | diff --git a/system/system_diagnostic_graph/doc/format/unit.md b/system/system_diagnostic_graph/doc/format/unit.md new file mode 100644 index 0000000000..791689aa2d --- /dev/null +++ b/system/system_diagnostic_graph/doc/format/unit.md @@ -0,0 +1,10 @@ +# Unit + +Diag is a node that refers to a functional unit. + +## Format + +| Name | Type | Required | Description | +| ---- | ------ | -------- | -------------------------------------- | +| type | string | yes | Specify `unit` when using this object. | +| name | string | yes | Name of diagnostic status. | diff --git a/system/system_diagnostic_graph/doc/overview.drawio.svg b/system/system_diagnostic_graph/doc/overview.drawio.svg new file mode 100644 index 0000000000..aa6be5a48b --- /dev/null +++ b/system/system_diagnostic_graph/doc/overview.drawio.svg @@ -0,0 +1,391 @@ + + + + + + + +
+
+
+ /diagnostics_graph_status +
+
+
+ + /diagnostics_graph_status + + + + + + +
+
+
+ /diagnostics +
+
+
+ + /diagnostics + + + + + + + + +
+
+
+ Top LiDAR +
+
+
+ + Top LiDAR + + + + + + + + + + +
+
+
+ Front LiDAR +
+
+
+ + Front LiDAR + + + + + + + + + + +
+
+
+ Front obstacle detection +
+
+
+ + Front obstacle detec... + + + + + + + + +
+
+
+ Pose estimation +
+
+
+ + Pose estimation + + + + + + + + +
+
+
+ Autonomous mode +
+
+
+ + Autonomous mode + + + + + + + + +
+
+
+ Comfortable stop +
+
+
+ + Comfortable stop + + + + + + + + +
+
+
+ Emergncy stop +
+
+
+ + Emergncy stop + + + + + + + + +
+
+
+ Route +
+
+
+ + Route + + + + + + + + +
+
+
+ Joystick mode +
+
+
+ + Joystick mode + + + + + + + + +
+
+
+ Joystick +
+
+
+ + Joystick + + + + + + +
+
+
+ AND +
+
+
+ + AND + + + + + + + + +
+
+
+ Filter by use case and system state +
+
+
+ + Filter by use case and system state + + + + + + + + +
+
+
+ Stop mode +
+
+
+ + Stop mode + + + + + + +
+
+
+ Error report +
+
+
+ + Error report + + + + + + + + +
+
+
+ Front radar +
+
+
+ + Front radar + + + + + + +
+
+
+ OR +
+
+
+ + OR + + + + + + + + +
+
+
+ Front obstacle prediction +
+
+
+ + Front obstacle predi... + + + + + + + Text is not SVG - cannot display + + + diff --git a/system/system_diagnostic_graph/example/example.launch.xml b/system/system_diagnostic_graph/example/example.launch.xml new file mode 100644 index 0000000000..461842f020 --- /dev/null +++ b/system/system_diagnostic_graph/example/example.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/system_diagnostic_graph/example/example1.yaml b/system/system_diagnostic_graph/example/example1.yaml new file mode 100644 index 0000000000..25053b5f7e --- /dev/null +++ b/system/system_diagnostic_graph/example/example1.yaml @@ -0,0 +1,41 @@ +files: + - { package: system_diagnostic_graph, path: example/example2.yaml } + +nodes: + - name: /autoware/diagnostics + type: and + list: + - { type: unit, name: /autoware/operation/stop } + - { type: unit, name: /autoware/operation/autonomous } + - { type: unit, name: /autoware/operation/local } + - { type: unit, name: /autoware/operation/remote } + - { type: unit, name: /autoware/operation/emergency-stop } + - { type: unit, name: /autoware/operation/comfortable-stop } + + - name: /autoware/operation/stop + type: debug-ok + + - name: /autoware/operation/autonomous + type: and + list: + - { type: unit, name: /autoware/localization } + - { type: unit, name: /autoware/planning } + - { type: unit, name: /autoware/perception } + + - name: /autoware/operation/local + type: debug-ok + + - name: /autoware/operation/remote + type: and + list: + - { type: diag, hardware: test, name: /external/remote_command } + + - name: /autoware/operation/emergency-stop + type: debug-ok + + - name: /autoware/operation/comfortable-stop + type: and + list: + - { type: unit, name: /autoware/localization } + - { type: unit, name: /autoware/planning } + - { type: unit, name: /autoware/perception/front-obstacle-detection } diff --git a/system/system_diagnostic_graph/example/example2.yaml b/system/system_diagnostic_graph/example/example2.yaml new file mode 100644 index 0000000000..2af09c0cb6 --- /dev/null +++ b/system/system_diagnostic_graph/example/example2.yaml @@ -0,0 +1,32 @@ +nodes: + - name: /autoware/localization + type: and + list: + - { type: diag, hardware: test, name: /sensing/lidars/top } + + - name: /autoware/planning + type: and + list: + - { type: unit, name: /autoware/planning/route } + + - name: /autoware/planning/route + type: and + list: + - { type: diag, hardware: test, name: /planning/route } + + - name: /autoware/perception + type: and + list: + - { type: unit, name: /autoware/perception/front-obstacle-detection } + - { type: unit, name: /autoware/perception/front-obstacle-prediction } + + - name: /autoware/perception/front-obstacle-detection + type: or + list: + - { type: diag, hardware: test, name: /sensing/lidars/front } + - { type: diag, hardware: test, name: /sensing/radars/front } + + - name: /autoware/perception/front-obstacle-prediction + type: and + list: + - { type: diag, hardware: test, name: /sensing/lidars/front } diff --git a/system/system_diagnostic_graph/example/publisher.py b/system/system_diagnostic_graph/example/publisher.py new file mode 100755 index 0000000000..2e73237284 --- /dev/null +++ b/system/system_diagnostic_graph/example/publisher.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# 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. + +import argparse + +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos + + +class DummyDiagnostics(rclpy.node.Node): + def __init__(self, array): + super().__init__("dummy_diagnostics") + qos = rclpy.qos.qos_profile_system_default + self.diags = self.create_publisher(DiagnosticArray, "/diagnostics", qos) + self.timer = self.create_timer(0.5, self.on_timer) + self.array = [self.create_status(*data) for data in array] + + def on_timer(self): + diagnostics = DiagnosticArray() + diagnostics.header.stamp = self.get_clock().now().to_msg() + diagnostics.status = self.array + self.diags.publish(diagnostics) + + @staticmethod + def create_status(name: str, level: int): + return DiagnosticStatus(level=level, name=name, message="OK", hardware_id="test") + + +if __name__ == "__main__": + data = { + "ok": [ + ("/sensing/lidars/top", DiagnosticStatus.OK), + ("/sensing/lidars/front", DiagnosticStatus.OK), + ("/sensing/radars/front", DiagnosticStatus.OK), + ("/planning/route", DiagnosticStatus.OK), + ("/external/remote_command", DiagnosticStatus.OK), + ], + "front-lidar": [ + ("/sensing/lidars/top", DiagnosticStatus.OK), + ("/sensing/lidars/front", DiagnosticStatus.ERROR), + ("/sensing/radars/front", DiagnosticStatus.OK), + ("/planning/route", DiagnosticStatus.OK), + ("/external/remote_command", DiagnosticStatus.OK), + ], + "front-radar": [ + ("/sensing/lidars/top", DiagnosticStatus.OK), + ("/sensing/lidars/front", DiagnosticStatus.OK), + ("/sensing/radars/front", DiagnosticStatus.ERROR), + ("/planning/route", DiagnosticStatus.OK), + ("/external/remote_command", DiagnosticStatus.OK), + ], + "front": [ + ("/sensing/lidars/top", DiagnosticStatus.OK), + ("/sensing/lidars/front", DiagnosticStatus.ERROR), + ("/sensing/radars/front", DiagnosticStatus.ERROR), + ("/planning/route", DiagnosticStatus.OK), + ("/external/remote_command", DiagnosticStatus.OK), + ], + } + + parser = argparse.ArgumentParser() + parser.add_argument("--data", default="ok") + args = parser.parse_args() + + rclpy.init() + rclpy.spin(DummyDiagnostics(data[args.data])) + rclpy.shutdown() diff --git a/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml b/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml new file mode 100644 index 0000000000..5b9a84947d --- /dev/null +++ b/system/system_diagnostic_graph/launch/system_diagnostic_graph.launch.xml @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/system/system_diagnostic_graph/package.xml b/system/system_diagnostic_graph/package.xml new file mode 100644 index 0000000000..ffc0c6c7d5 --- /dev/null +++ b/system/system_diagnostic_graph/package.xml @@ -0,0 +1,24 @@ + + + + system_diagnostic_graph + 0.1.0 + The system_diagnostic_graph package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + diagnostic_msgs + rclcpp + tier4_system_msgs + yaml_cpp_vendor + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp new file mode 100644 index 0000000000..b33a7deb66 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -0,0 +1,122 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "config.hpp" + +#include + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +ConfigError create_error(const FileConfig & config, const std::string & message) +{ + const std::string marker = config ? "File:" + config->path : "Parameter"; + return ConfigError(message + " (" + marker + ")"); +} + +ConfigError create_error(const NodeConfig & config, const std::string & message) +{ + const std::string marker = "File:" + config->path + ", Node:" + config->name; + return ConfigError(message + " (" + marker + ")"); +} + +NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope) +{ + if (!yaml.IsMap()) { + throw create_error(scope, "node object is not a dict"); + } + if (!yaml["name"]) { + throw create_error(scope, "node object has no 'name' field"); + } + + const auto config = std::make_shared(); + config->path = scope->path; + config->name = take(yaml, "name"); + config->yaml = yaml; + return config; +} + +FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope) +{ + if (!yaml.IsMap()) { + throw create_error(scope, "file object is not a dict"); + } + if (!yaml["package"]) { + throw create_error(scope, "file object has no 'package' field"); + } + if (!yaml["path"]) { + throw create_error(scope, "file object has no 'path' field"); + } + + const auto package_name = yaml["package"].as(); + const auto package_path = ament_index_cpp::get_package_share_directory(package_name); + return parse_config_path(package_path + "/" + yaml["path"].as(), scope); +} + +FileConfig parse_config_path(const std::string & path, const FileConfig & scope) +{ + if (!std::filesystem::exists(path)) { + throw create_error(scope, "config file '" + path + "' does not exist"); + } + return parse_config_file(path); +} + +FileConfig parse_config_file(const std::string & path) +{ + const auto config = std::make_shared(); + config->path = path; + + const auto yaml = YAML::LoadFile(path); + if (!yaml.IsMap()) { + throw create_error(config, "config file is not a dict"); + } + + const auto files = yaml["files"] ? yaml["files"] : YAML::Node(YAML::NodeType::Sequence); + if (!files.IsSequence()) { + throw create_error(config, "files section is not a list"); + } + for (const auto file : files) { + config->files.push_back(parse_config_path(file, config)); + } + + const auto nodes = yaml["nodes"] ? yaml["nodes"] : YAML::Node(YAML::NodeType::Sequence); + if (!nodes.IsSequence()) { + throw create_error(config, "nodes section is not a list"); + } + for (const auto node : nodes) { + config->nodes.push_back(parse_config_node(node, config)); + } + + return config; +} + +void walk_config_tree(const FileConfig & config, std::vector & nodes) +{ + nodes.insert(nodes.end(), config->nodes.begin(), config->nodes.end()); + for (const auto & file : config->files) walk_config_tree(file, nodes); +} + +std::vector load_config_file(const std::string & path) +{ + std::vector nodes; + walk_config_tree(parse_config_path(path, nullptr), nodes); + return nodes; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp new file mode 100644 index 0000000000..6393cb906b --- /dev/null +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -0,0 +1,68 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__CONFIG_HPP_ +#define CORE__CONFIG_HPP_ + +#include + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +struct ConfigError : public std::runtime_error +{ + using runtime_error::runtime_error; +}; + +struct NodeConfig_ +{ + std::string path; + std::string name; + YAML::Node yaml; +}; + +struct FileConfig_ +{ + std::string path; + std::vector> files; + std::vector> nodes; +}; + +template +T take(YAML::Node yaml, const std::string & field) +{ + const auto result = yaml[field].as(); + yaml.remove(field); + return result; +} + +using NodeConfig = std::shared_ptr; +using FileConfig = std::shared_ptr; +ConfigError create_error(const FileConfig & config, const std::string & message); +ConfigError create_error(const NodeConfig & config, const std::string & message); +std::vector load_config_file(const std::string & path); + +NodeConfig parse_config_node(YAML::Node yaml, const FileConfig & scope); +FileConfig parse_config_path(YAML::Node yaml, const FileConfig & scope); +FileConfig parse_config_path(const std::string & path, const FileConfig & scope); +FileConfig parse_config_file(const std::string & path); + +} // namespace system_diagnostic_graph + +#endif // CORE__CONFIG_HPP_ diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp new file mode 100644 index 0000000000..337be8c74a --- /dev/null +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -0,0 +1,75 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "debug.hpp" + +#include "node.hpp" +#include "types.hpp" +#include "update.hpp" + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +const std::unordered_map level_names = { + {DiagnosticStatus::OK, "OK"}, + {DiagnosticStatus::WARN, "WARN"}, + {DiagnosticStatus::ERROR, "ERROR"}, + {DiagnosticStatus::STALE, "STALE"}}; + +void DiagGraph::debug() +{ + std::vector lines; + for (const auto & node : graph_.nodes()) { + lines.push_back(node->debug()); + } + + std::array widths = {}; + for (const auto & line : lines) { + for (size_t i = 0; i < diag_debug_size; ++i) { + widths[i] = std::max(widths[i], line[i].length()); + } + } + + const size_t total_width = std::accumulate(widths.begin(), widths.end(), 0); + std::cout << std::string(total_width + 3 * diag_debug_size + 1, '=') << std::endl; + + for (const auto & line : lines) { + for (size_t i = 0; i < diag_debug_size; ++i) { + std::cout << "| " << std::left << std::setw(widths[i]) << line[i] << " "; + } + std::cout << "|" << std::endl; + } +} + +DiagDebugData UnitNode::debug() const +{ + const auto & level = node_.status.level; + const auto & name = node_.status.name; + return DiagDebugData{std::to_string(index()), "unit", name, "-----", level_names.at(level)}; +} + +DiagDebugData DiagNode::debug() const +{ + const auto & level = node_.status.level; + const auto & name = node_.status.name; + const auto & hardware = node_.status.hardware_id; + return DiagDebugData{std::to_string(index()), "diag", name, hardware, level_names.at(level)}; +} + +} // namespace system_diagnostic_graph diff --git a/common/motion_utils/src/motion_utils.cpp b/system/system_diagnostic_graph/src/core/debug.hpp similarity index 62% rename from common/motion_utils/src/motion_utils.cpp rename to system/system_diagnostic_graph/src/core/debug.hpp index e1c86d1ad2..bb1bbc6f14 100644 --- a/common/motion_utils/src/motion_utils.cpp +++ b/system/system_diagnostic_graph/src/core/debug.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 Tier IV, Inc. +// Copyright 2023 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,4 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion_utils/motion_utils.hpp" +#ifndef CORE__DEBUG_HPP_ +#define CORE__DEBUG_HPP_ + +#include +#include + +namespace system_diagnostic_graph +{ + +constexpr size_t diag_debug_size = 5; +using DiagDebugData = std::array; + +} // namespace system_diagnostic_graph + +#endif // CORE__DEBUG_HPP_ diff --git a/system/system_diagnostic_graph/src/core/expr.cpp b/system/system_diagnostic_graph/src/core/expr.cpp new file mode 100644 index 0000000000..dc7ebcf8dd --- /dev/null +++ b/system/system_diagnostic_graph/src/core/expr.cpp @@ -0,0 +1,284 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "expr.hpp" + +#include "config.hpp" +#include "graph.hpp" +#include "node.hpp" + +#include +#include +#include +#include + +// +#include + +namespace system_diagnostic_graph +{ + +using LinkStatus = std::vector>; + +void extend(LinkStatus & a, const LinkStatus & b) +{ + a.insert(a.end(), b.begin(), b.end()); +} + +void extend_false(LinkStatus & a, const LinkStatus & b) +{ + for (const auto & p : b) { + a.push_back(std::make_pair(p.first, false)); + } +} + +std::unique_ptr BaseExpr::create(Graph & graph, YAML::Node yaml) +{ + if (!yaml.IsMap()) { + throw ConfigError("expr object is not a dict"); + } + if (!yaml["type"]) { + throw ConfigError("expr object has no 'type' field"); + } + + const auto type = take(yaml, "type"); + + if (type == "unit") { + return std::make_unique(graph, yaml); + } + if (type == "diag") { + return std::make_unique(graph, yaml); + } + if (type == "and") { + return std::make_unique(graph, yaml); + } + if (type == "or") { + return std::make_unique(graph, yaml); + } + if (type == "if") { + return std::make_unique(graph, yaml); + } + if (type == "debug-ok") { + return std::make_unique(DiagnosticStatus::OK); + } + if (type == "debug-warn") { + return std::make_unique(DiagnosticStatus::WARN); + } + if (type == "debug-error") { + return std::make_unique(DiagnosticStatus::ERROR); + } + if (type == "debug-stale") { + return std::make_unique(DiagnosticStatus::STALE); + } + throw ConfigError("unknown expr type: " + type); +} + +ConstExpr::ConstExpr(const DiagnosticLevel level) +{ + level_ = level; +} + +ExprStatus ConstExpr::eval() const +{ + ExprStatus status; + status.level = level_; + return status; +} + +std::vector ConstExpr::get_dependency() const +{ + return {}; +} + +UnitExpr::UnitExpr(Graph & graph, YAML::Node yaml) +{ + if (!yaml["name"]) { + throw ConfigError("unit object has no 'name' field"); + } + const auto name = take(yaml, "name"); + node_ = graph.find_unit(name); + if (!node_) { + throw ConfigError("unit node '" + name + "' does not exist"); + } +} + +ExprStatus UnitExpr::eval() const +{ + ExprStatus status; + status.level = node_->level(); + status.links.push_back(std::make_pair(node_, true)); + return status; +} + +std::vector UnitExpr::get_dependency() const +{ + return {node_}; +} + +DiagExpr::DiagExpr(Graph & graph, YAML::Node yaml) +{ + if (!yaml["name"]) { + throw ConfigError("diag object has no 'name' field"); + } + const auto name = yaml["name"].as(); + const auto hardware = yaml["hardware"].as(""); + node_ = graph.find_diag(name, hardware); + if (!node_) { + node_ = graph.make_diag(name, hardware); + } +} + +ExprStatus DiagExpr::eval() const +{ + ExprStatus status; + status.level = node_->level(); + status.links.push_back(std::make_pair(node_, true)); + return status; +} + +std::vector DiagExpr::get_dependency() const +{ + return {node_}; +} + +AndExpr::AndExpr(Graph & graph, YAML::Node yaml) +{ + if (!yaml["list"]) { + throw ConfigError("expr object has no 'list' field"); + } + if (!yaml["list"].IsSequence()) { + throw ConfigError("list field is not a list"); + } + + for (const auto & node : yaml["list"]) { + list_.push_back(BaseExpr::create(graph, node)); + } +} + +ExprStatus AndExpr::eval() const +{ + std::vector results; + for (const auto & expr : list_) { + results.push_back(expr->eval()); + } + std::vector levels; + for (const auto & result : results) { + levels.push_back(result.level); + } + ExprStatus status; + for (const auto & result : results) { + extend(status.links, result.links); + } + const auto level = *std::max_element(levels.begin(), levels.end()); + status.level = std::min(level, DiagnosticStatus::ERROR); + return status; +} + +std::vector AndExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +OrExpr::OrExpr(Graph & graph, YAML::Node yaml) +{ + if (!yaml["list"]) { + throw ConfigError("expr object has no 'list' field"); + } + if (!yaml["list"].IsSequence()) { + throw ConfigError("list field is not a list"); + } + + for (const auto & node : yaml["list"]) { + list_.push_back(BaseExpr::create(graph, node)); + } +} + +ExprStatus OrExpr::eval() const +{ + std::vector results; + for (const auto & expr : list_) { + results.push_back(expr->eval()); + } + std::vector levels; + for (const auto & result : results) { + levels.push_back(result.level); + } + ExprStatus status; + for (const auto & result : results) { + extend(status.links, result.links); + } + const auto level = *std::min_element(levels.begin(), levels.end()); + status.level = std::min(level, DiagnosticStatus::ERROR); + return status; +} + +std::vector OrExpr::get_dependency() const +{ + std::vector depends; + for (const auto & expr : list_) { + const auto nodes = expr->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +IfExpr::IfExpr(Graph & graph, YAML::Node yaml) +{ + if (!yaml["cond"]) { + throw ConfigError("expr object has no 'cond' field"); + } + if (!yaml["then"]) { + throw ConfigError("expr object has no 'then' field"); + } + cond_ = BaseExpr::create(graph, yaml["cond"]); + then_ = BaseExpr::create(graph, yaml["then"]); +} + +ExprStatus IfExpr::eval() const +{ + const auto cond = cond_->eval(); + const auto then = then_->eval(); + ExprStatus status; + if (cond.level == DiagnosticStatus::OK) { + status.level = std::min(then.level, DiagnosticStatus::ERROR); + extend(status.links, cond.links); + extend(status.links, then.links); + } else { + status.level = std::min(cond.level, DiagnosticStatus::ERROR); + extend(status.links, cond.links); + extend_false(status.links, then.links); + } + return status; +} + +std::vector IfExpr::get_dependency() const +{ + std::vector depends; + { + const auto nodes = cond_->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + { + const auto nodes = then_->get_dependency(); + depends.insert(depends.end(), nodes.begin(), nodes.end()); + } + return depends; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/expr.hpp b/system/system_diagnostic_graph/src/core/expr.hpp new file mode 100644 index 0000000000..5418415821 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/expr.hpp @@ -0,0 +1,114 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__EXPR_HPP_ +#define CORE__EXPR_HPP_ + +#include "types.hpp" + +#include + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +struct ExprStatus +{ + DiagnosticLevel level; + std::vector> links; +}; + +class BaseExpr +{ +public: + static std::unique_ptr create(Graph & graph, YAML::Node yaml); + virtual ~BaseExpr() = default; + virtual ExprStatus eval() const = 0; + virtual std::vector get_dependency() const = 0; +}; + +class ConstExpr : public BaseExpr +{ +public: + explicit ConstExpr(const DiagnosticLevel level); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + DiagnosticLevel level_; +}; + +class UnitExpr : public BaseExpr +{ +public: + UnitExpr(Graph & graph, YAML::Node yaml); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + UnitNode * node_; +}; + +class DiagExpr : public BaseExpr +{ +public: + DiagExpr(Graph & graph, YAML::Node yaml); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + DiagNode * node_; +}; + +class AndExpr : public BaseExpr +{ +public: + AndExpr(Graph & graph, YAML::Node yaml); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + std::vector> list_; +}; + +class OrExpr : public BaseExpr +{ +public: + OrExpr(Graph & graph, YAML::Node yaml); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + std::vector> list_; +}; + +class IfExpr : public BaseExpr +{ +public: + IfExpr(Graph & graph, YAML::Node yaml); + ExprStatus eval() const override; + std::vector get_dependency() const override; + +private: + std::unique_ptr cond_; + std::unique_ptr then_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__EXPR_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp new file mode 100644 index 0000000000..ba0e3cfd2e --- /dev/null +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -0,0 +1,118 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "graph.hpp" + +#include "node.hpp" + +#include +#include +#include +#include + +// +#include + +namespace system_diagnostic_graph +{ + +UnitNode * Graph::make_unit(const std::string & name) +{ + const auto key = name; + auto unit = std::make_unique(key); + units_[key] = unit.get(); + nodes_.emplace_back(std::move(unit)); + return units_[key]; +} + +UnitNode * Graph::find_unit(const std::string & name) +{ + const auto key = name; + return units_.count(key) ? units_.at(key) : nullptr; +} + +DiagNode * Graph::make_diag(const std::string & name, const std::string & hardware) +{ + const auto key = std::make_pair(name, hardware); + auto diag = std::make_unique(name, hardware); + diags_[key] = diag.get(); + nodes_.emplace_back(std::move(diag)); + return diags_[key]; +} + +DiagNode * Graph::find_diag(const std::string & name, const std::string & hardware) +{ + const auto key = std::make_pair(name, hardware); + return diags_.count(key) ? diags_.at(key) : nullptr; +} + +void Graph::topological_sort() +{ + std::map degrees; + std::deque nodes; + std::deque result; + std::deque buffer; + + // Create a list of raw pointer nodes. + for (const auto & node : nodes_) { + nodes.push_back(node.get()); + } + + // Count degrees of each node. + for (const auto & node : nodes) { + for (const auto & link : node->links()) { + ++degrees[link]; + } + } + + // Find initial nodes that are zero degrees. + for (const auto & node : nodes) { + if (degrees[node] == 0) { + buffer.push_back(node); + } + } + + // Sort by topological order. + while (!buffer.empty()) { + const auto node = buffer.front(); + buffer.pop_front(); + for (const auto & link : node->links()) { + if (--degrees[link] == 0) { + buffer.push_back(link); + } + } + result.push_back(node); + } + + // Detect circulation because the result does not include the nodes on the loop. + if (result.size() != nodes.size()) { + throw ConfigError("detect graph circulation"); + } + + // Reverse the result to process from leaf node. + result = std::deque(result.rbegin(), result.rend()); + + // Replace node vector. + std::map indices; + for (size_t i = 0; i < result.size(); ++i) { + indices[result[i]] = i; + } + std::vector> temp(nodes_.size()); + for (auto & node : nodes_) { + temp[indices[node.get()]] = std::move(node); + } + nodes_ = std::move(temp); +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp new file mode 100644 index 0000000000..a3f4676038 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -0,0 +1,47 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__GRAPH_HPP_ +#define CORE__GRAPH_HPP_ + +#include "types.hpp" + +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class Graph +{ +public: + UnitNode * make_unit(const std::string & name); + UnitNode * find_unit(const std::string & name); + DiagNode * make_diag(const std::string & name, const std::string & hardware); + DiagNode * find_diag(const std::string & name, const std::string & hardware); + void topological_sort(); + const std::vector> & nodes() { return nodes_; } + +private: + std::vector> nodes_; + std::map units_; + std::map, DiagNode *> diags_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__GRAPH_HPP_ diff --git a/system/system_diagnostic_graph/src/core/node.cpp b/system/system_diagnostic_graph/src/core/node.cpp new file mode 100644 index 0000000000..1439188d93 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/node.cpp @@ -0,0 +1,94 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "node.hpp" + +#include "expr.hpp" + +#include + +#include +#include + +namespace system_diagnostic_graph +{ + +UnitNode::UnitNode(const std::string & name) +{ + node_.status.level = DiagnosticStatus::STALE; + node_.status.name = name; + node_.status.hardware_id = ""; +} + +UnitNode::~UnitNode() +{ + // for unique_ptr +} + +DiagnosticNode UnitNode::report() const +{ + return node_; +} + +void UnitNode::update() +{ + const auto result = expr_->eval(); + node_.status.level = result.level; + node_.links.clear(); + for (const auto & [node, used] : result.links) { + DiagnosticLink link; + link.index = node->index(); + link.used = used; + node_.links.push_back(link); + } +} + +void UnitNode::create(Graph & graph, const NodeConfig & config) +{ + try { + expr_ = BaseExpr::create(graph, config->yaml); + } catch (const ConfigError & error) { + throw create_error(config, error.what()); + } +} + +std::vector UnitNode::links() const +{ + return expr_->get_dependency(); +} + +DiagNode::DiagNode(const std::string & name, const std::string & hardware) +{ + node_.status.level = DiagnosticStatus::STALE; + node_.status.name = name; + node_.status.hardware_id = hardware; +} + +DiagnosticNode DiagNode::report() const +{ + return node_; +} + +void DiagNode::update() +{ + // TODO(Takagi, Isamu): timeout, error hold + // constexpr double timeout = 1.0; // TODO(Takagi, Isamu): parameterize +} + +void DiagNode::callback(const DiagnosticStatus & status) +{ + node_.status = status; +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/node.hpp b/system/system_diagnostic_graph/src/core/node.hpp new file mode 100644 index 0000000000..359153fc28 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/node.hpp @@ -0,0 +1,84 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__NODE_HPP_ +#define CORE__NODE_HPP_ + +#include "config.hpp" +#include "debug.hpp" +#include "types.hpp" + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseNode +{ +public: + virtual ~BaseNode() = default; + virtual void update() = 0; + virtual DiagnosticNode report() const = 0; + virtual DiagDebugData debug() const = 0; + virtual std::vector links() const = 0; + + DiagnosticLevel level() const { return node_.status.level; } + std::string name() const { return node_.status.name; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + size_t index_ = 0; + DiagnosticNode node_; +}; + +class UnitNode : public BaseNode +{ +public: + explicit UnitNode(const std::string & name); + ~UnitNode() override; + + DiagnosticNode report() const override; + DiagDebugData debug() const override; + void update() override; + void create(Graph & graph, const NodeConfig & config); + + std::vector links() const override; + +private: + std::unique_ptr expr_; +}; + +class DiagNode : public BaseNode +{ +public: + explicit DiagNode(const std::string & name, const std::string & hardware); + + DiagnosticNode report() const override; + DiagDebugData debug() const override; + void update() override; + void callback(const DiagnosticStatus & status); + + std::vector links() const override { return {}; } + +private: +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__NODE_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp new file mode 100644 index 0000000000..75167958e4 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -0,0 +1,45 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__TYPES_HPP_ +#define CORE__TYPES_HPP_ + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +using diagnostic_msgs::msg::DiagnosticArray; +using diagnostic_msgs::msg::DiagnosticStatus; +using tier4_system_msgs::msg::DiagnosticGraph; +using tier4_system_msgs::msg::DiagnosticLink; +using tier4_system_msgs::msg::DiagnosticNode; +using tier4_system_msgs::msg::OperationModeAvailability; + +using DiagnosticLevel = DiagnosticStatus::_level_type; + +class Graph; +class BaseNode; +class UnitNode; +class DiagNode; +class BaseExpr; + +} // namespace system_diagnostic_graph + +#endif // CORE__TYPES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/update.cpp b/system/system_diagnostic_graph/src/core/update.cpp new file mode 100644 index 0000000000..bb42dcba12 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/update.cpp @@ -0,0 +1,116 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "update.hpp" + +#include "config.hpp" + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +UnitNode * find_node(Graph & graph, const std::string & name) +{ + const auto node = graph.find_unit(name); + if (!node) { + throw ConfigError("summary node '" + name + "' does node exist"); + } + return node; +}; + +void DiagGraph::create(const std::string & file) +{ + const auto configs = load_config_file(file); + + // Create unit nodes first because it is necessary for the link. + std::vector> units; + for (const auto & config : configs) { + UnitNode * unit = graph_.make_unit(config->name); + units.push_back(std::make_pair(config, unit)); + } + + // Reflect the config after creating all the unit nodes, + for (auto & [config, unit] : units) { + unit->create(graph_, config); + } + + // Sort unit nodes in topological order for update dependencies. + graph_.topological_sort(); + + // Set the link index for the ros message. + const auto & nodes = graph_.nodes(); + for (size_t i = 0; i < nodes.size(); ++i) { + nodes[i]->set_index(i); + } + + // Get reserved unit node for summary. + summary_.stop_mode = find_node(graph_, "/autoware/operation/stop"); + summary_.autonomous_mode = find_node(graph_, "/autoware/operation/autonomous"); + summary_.local_mode = find_node(graph_, "/autoware/operation/local"); + summary_.remote_mode = find_node(graph_, "/autoware/operation/remote"); + summary_.emergency_stop_mrm = find_node(graph_, "/autoware/operation/emergency-stop"); + summary_.comfortable_stop_mrm = find_node(graph_, "/autoware/operation/comfortable-stop"); + summary_.pull_over_mrm = find_node(graph_, "/autoware/operation/pull-over"); +} + +DiagnosticGraph DiagGraph::report(const rclcpp::Time & stamp) +{ + DiagnosticGraph message; + message.stamp = stamp; + message.nodes.reserve(graph_.nodes().size()); + + for (const auto & node : graph_.nodes()) { + node->update(); + } + for (const auto & node : graph_.nodes()) { + message.nodes.push_back(node->report()); + } + return message; +} + +OperationModeAvailability DiagGraph::summary(const rclcpp::Time & stamp) +{ + const auto is_ok = [](const UnitNode * node) { return node->level() == DiagnosticStatus::OK; }; + + OperationModeAvailability message; + message.stamp = stamp; + message.stop = is_ok(summary_.stop_mode); + message.autonomous = is_ok(summary_.autonomous_mode); + message.local = is_ok(summary_.local_mode); + message.remote = is_ok(summary_.remote_mode); + message.emergency_stop = is_ok(summary_.emergency_stop_mrm); + message.comfortable_stop = is_ok(summary_.comfortable_stop_mrm); + message.pull_over = is_ok(summary_.pull_over_mrm); + return message; +} + +void DiagGraph::callback(const DiagnosticArray & array) +{ + for (const auto & status : array.status) { + auto diag = graph_.find_diag(status.name, status.hardware_id); + if (diag) { + diag->callback(status); + } else { + // TODO(Takagi, Isamu): handle unknown diagnostics + } + } +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/update.hpp b/system/system_diagnostic_graph/src/core/update.hpp new file mode 100644 index 0000000000..3cba96ad82 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/update.hpp @@ -0,0 +1,58 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 CORE__UPDATE_HPP_ +#define CORE__UPDATE_HPP_ + +#include "graph.hpp" +#include "node.hpp" +#include "types.hpp" + +#include + +#include +#include + +namespace system_diagnostic_graph +{ + +struct Summary +{ + UnitNode * stop_mode; + UnitNode * autonomous_mode; + UnitNode * local_mode; + UnitNode * remote_mode; + UnitNode * emergency_stop_mrm; + UnitNode * comfortable_stop_mrm; + UnitNode * pull_over_mrm; +}; + +class DiagGraph +{ +public: + void create(const std::string & file); + void callback(const DiagnosticArray & array); + DiagnosticGraph report(const rclcpp::Time & stamp); + OperationModeAvailability summary(const rclcpp::Time & stamp); + + void debug(); + +private: + Graph graph_; + Summary summary_; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__UPDATE_HPP_ diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp new file mode 100644 index 0000000000..38b9fa5bac --- /dev/null +++ b/system/system_diagnostic_graph/src/main.cpp @@ -0,0 +1,74 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "main.hpp" + +#include +#include + +namespace system_diagnostic_graph +{ + +MainNode::MainNode() : Node("system_diagnostic_graph_aggregator") +{ + // Init ros interface. + { + using std::placeholders::_1; + const auto qos_input = rclcpp::QoS(declare_parameter("input_qos_depth")); + const auto qos_graph = rclcpp::QoS(declare_parameter("graph_qos_depth")); + + const auto callback = std::bind(&MainNode::on_diag, this, _1); + sub_input_ = create_subscription("/diagnostics", qos_input, callback); + pub_graph_ = create_publisher("/diagnostics_graph", qos_graph); + pub_modes_ = create_publisher( + "/system/operation_mode/availability", rclcpp::QoS(1)); + + const auto rate = rclcpp::Rate(declare_parameter("rate")); + timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); + } + + // Init diagnostics graph. + { + const auto file = declare_parameter("graph_file"); + graph_.create(file); + graph_.debug(); + } +} + +void MainNode::on_timer() +{ + const auto data = graph_.report(now()); + graph_.debug(); + pub_graph_->publish(data); + pub_modes_->publish(graph_.summary(now())); +} + +void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) +{ + graph_.callback(*msg); +} + +} // namespace system_diagnostic_graph + +int main(int argc, char ** argv) +{ + using system_diagnostic_graph::MainNode; + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/system/system_diagnostic_graph/src/main.hpp b/system/system_diagnostic_graph/src/main.hpp new file mode 100644 index 0000000000..4cc659c978 --- /dev/null +++ b/system/system_diagnostic_graph/src/main.hpp @@ -0,0 +1,43 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 MAIN_HPP_ +#define MAIN_HPP_ + +#include "core/types.hpp" +#include "core/update.hpp" + +#include + +namespace system_diagnostic_graph +{ + +class MainNode : public rclcpp::Node +{ +public: + MainNode(); + +private: + DiagGraph graph_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr sub_input_; + rclcpp::Publisher::SharedPtr pub_graph_; + rclcpp::Publisher::SharedPtr pub_modes_; + void on_timer(); + void on_diag(const DiagnosticArray::ConstSharedPtr msg); +}; + +} // namespace system_diagnostic_graph + +#endif // MAIN_HPP_ diff --git a/system/system_diagnostic_graph/src/tool.cpp b/system/system_diagnostic_graph/src/tool.cpp new file mode 100644 index 0000000000..fda2c88a69 --- /dev/null +++ b/system/system_diagnostic_graph/src/tool.cpp @@ -0,0 +1,81 @@ +// Copyright 2023 The Autoware Contributors +// +// 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 "tool.hpp" + +#include +#include + +namespace system_diagnostic_graph +{ + +std::string level_to_string(DiagnosticLevel level) +{ + switch (level) { + case DiagnosticStatus::OK: + return "OK"; + case DiagnosticStatus::WARN: + return "WARN"; + case DiagnosticStatus::ERROR: + return "ERROR"; + case DiagnosticStatus::STALE: + return "STALE"; + } + return "UNKNOWN"; +} + +ToolNode::ToolNode() : Node("system_diagnostic_graph_converter") +{ + using std::placeholders::_1; + const auto qos_graph = rclcpp::QoS(1); + const auto qos_array = rclcpp::QoS(1); + + const auto callback = std::bind(&ToolNode::on_graph, this, _1); + sub_graph_ = create_subscription("/diagnostics_graph", qos_graph, callback); + pub_array_ = create_publisher("/diagnostics_array", qos_array); +} + +void ToolNode::on_graph(const DiagnosticGraph::ConstSharedPtr msg) +{ + DiagnosticArray message; + message.header.stamp = msg->stamp; + message.status.reserve(msg->nodes.size()); + for (const auto & node : msg->nodes) { + message.status.push_back(node.status); + for (const auto & link : node.links) { + diagnostic_msgs::msg::KeyValue kv; + const auto & status = msg->nodes[link.index].status; + kv.key = status.name; + kv.value = level_to_string(status.level); + if (link.used) { + message.status.back().values.push_back(kv); + } + } + } + pub_array_->publish(message); +} + +} // namespace system_diagnostic_graph + +int main(int argc, char ** argv) +{ + using system_diagnostic_graph::ToolNode; + rclcpp::init(argc, argv); + rclcpp::executors::SingleThreadedExecutor executor; + auto node = std::make_shared(); + executor.add_node(node); + executor.spin(); + executor.remove_node(node); + rclcpp::shutdown(); +} diff --git a/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp b/system/system_diagnostic_graph/src/tool.hpp similarity index 51% rename from common/tier4_autoware_utils/src/tier4_autoware_utils.cpp rename to system/system_diagnostic_graph/src/tool.hpp index a541d314aa..11f6a26323 100644 --- a/common/tier4_autoware_utils/src/tier4_autoware_utils.cpp +++ b/system/system_diagnostic_graph/src/tool.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2023 The Autoware Contributors // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,4 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" +#ifndef TOOL_HPP_ +#define TOOL_HPP_ + +#include "core/graph.hpp" +#include "core/types.hpp" + +#include + +namespace system_diagnostic_graph +{ + +class ToolNode : public rclcpp::Node +{ +public: + ToolNode(); + +private: + rclcpp::Subscription::SharedPtr sub_graph_; + rclcpp::Publisher::SharedPtr pub_array_; + void on_graph(const DiagnosticGraph::ConstSharedPtr msg); +}; + +} // namespace system_diagnostic_graph + +#endif // TOOL_HPP_ diff --git a/system/system_diagnostic_graph/util/debug.py b/system/system_diagnostic_graph/util/debug.py new file mode 100755 index 0000000000..dac917a26a --- /dev/null +++ b/system/system_diagnostic_graph/util/debug.py @@ -0,0 +1,85 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# 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. + +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy +from rclpy.qos import QoSProfile +from tier4_system_msgs.msg import DiagnosticGraph + + +class StructNode(Node): + def __init__(self): + super().__init__("system_diagnostic_graph_tools_struct") + qos_struct = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) + self.sub_struct = self.create_subscription( + DiagnosticGraph, "/diagnostics_graph/struct", self.callback, qos_struct + ) + self.message = None + + def callback(self, msg): + self.message = msg + + +class NodeSpinner: + def __init__(self, time): + self.time = time + self.wait = True + + def on_timer(self): + self.wait = False + + def spin(self, node): + timer = node.create_timer(self.time, self.on_timer) + while self.wait: + rclpy.spin_once(node) + node.destroy_timer(timer) + + +class GraphNode: + def __init__(self, msg): + self.name = msg.name + self.links = msg.links + + +class GraphStruct: + def __init__(self, msg): + self.nodes = [GraphNode(node) for node in msg.nodes] + + @staticmethod + def Subscribe(): + spin = NodeSpinner(1.0) + node = StructNode() + spin.spin(node) + return GraphStruct(node.message) + + def visualize(self): + from graphviz import Digraph + + graph = Digraph() + graph.attr("node", shape="box") + for node in self.nodes: + graph.node(node.name) + for link in node.links: + graph.edge(node.name, self.nodes[link].name) + graph.view() + + +if __name__ == "__main__": + rclpy.init() + graph = GraphStruct.Subscribe() + rclpy.shutdown() + graph.visualize() diff --git a/system/system_diagnostic_monitor/CMakeLists.txt b/system/system_diagnostic_monitor/CMakeLists.txt new file mode 100644 index 0000000000..909210f99d --- /dev/null +++ b/system/system_diagnostic_monitor/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.14) +project(system_diagnostic_monitor) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_package(INSTALL_TO_SHARE config launch script) diff --git a/system/system_diagnostic_monitor/README.md b/system/system_diagnostic_monitor/README.md new file mode 100644 index 0000000000..8dccca34db --- /dev/null +++ b/system/system_diagnostic_monitor/README.md @@ -0,0 +1 @@ +# System diagnostic monitor diff --git a/system/system_diagnostic_monitor/config/autoware.yaml b/system/system_diagnostic_monitor/config/autoware.yaml new file mode 100644 index 0000000000..892a5da851 --- /dev/null +++ b/system/system_diagnostic_monitor/config/autoware.yaml @@ -0,0 +1,74 @@ +files: + - { package: system_diagnostic_monitor, path: config/map.yaml } + - { package: system_diagnostic_monitor, path: config/localization.yaml } + - { package: system_diagnostic_monitor, path: config/planning.yaml } + - { package: system_diagnostic_monitor, path: config/perception.yaml } + - { package: system_diagnostic_monitor, path: config/control.yaml } + - { package: system_diagnostic_monitor, path: config/vehicle.yaml } + - { package: system_diagnostic_monitor, path: config/system.yaml } + +nodes: + - name: /autoware + type: and + list: + - { type: unit, name: /autoware/operation/stop } + - { type: unit, name: /autoware/operation/autonomous } + - { type: unit, name: /autoware/operation/local } + - { type: unit, name: /autoware/operation/remote } + - { type: unit, name: /autoware/operation/emergency-stop } + - { type: unit, name: /autoware/operation/comfortable-stop } + - { type: unit, name: /autoware/operation/pull-over } + + - name: /autoware/operation/stop + type: debug-ok + + - name: /autoware/operation/autonomous + type: and + list: + - { type: unit, name: /autoware/map } + - { type: unit, name: /autoware/localization } + - { type: unit, name: /autoware/planning } + - { type: unit, name: /autoware/perception } + - { type: unit, name: /autoware/control } + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } + + - name: /autoware/operation/local + type: and + list: + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } + + - name: /autoware/operation/remote + type: and + list: + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } + + - name: /autoware/operation/emergency-stop + type: and + list: + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } + + - name: /autoware/operation/comfortable-stop + type: and + list: + - { type: unit, name: /autoware/map } + - { type: unit, name: /autoware/localization } + - { type: unit, name: /autoware/planning } + - { type: unit, name: /autoware/perception } + - { type: unit, name: /autoware/control } + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } + + - name: /autoware/operation/pull-over + type: and + list: + - { type: unit, name: /autoware/map } + - { type: unit, name: /autoware/localization } + - { type: unit, name: /autoware/planning } + - { type: unit, name: /autoware/perception } + - { type: unit, name: /autoware/control } + - { type: unit, name: /autoware/vehicle } + - { type: unit, name: /autoware/system } diff --git a/system/system_diagnostic_monitor/config/control.yaml b/system/system_diagnostic_monitor/config/control.yaml new file mode 100644 index 0000000000..8884a79847 --- /dev/null +++ b/system/system_diagnostic_monitor/config/control.yaml @@ -0,0 +1,10 @@ +nodes: + - name: /autoware/control + type: and + list: + - type: diag + name: "topic_state_monitor_trajectory_follower_control_cmd: control_topic_status" + hardware: topic_state_monitor + - type: diag + name: "topic_state_monitor_control_command_control_cmd: control_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/localization.yaml b/system/system_diagnostic_monitor/config/localization.yaml new file mode 100644 index 0000000000..26d680b6c4 --- /dev/null +++ b/system/system_diagnostic_monitor/config/localization.yaml @@ -0,0 +1,5 @@ +nodes: + - name: /autoware/localization + type: and + list: + - { type: diag, name: "component_state_diagnostics: localization_state" } diff --git a/system/system_diagnostic_monitor/config/map.yaml b/system/system_diagnostic_monitor/config/map.yaml new file mode 100644 index 0000000000..7bee741920 --- /dev/null +++ b/system/system_diagnostic_monitor/config/map.yaml @@ -0,0 +1,7 @@ +nodes: + - name: /autoware/map + type: and + list: + - type: diag + name: "topic_state_monitor_vector_map: map_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/perception.yaml b/system/system_diagnostic_monitor/config/perception.yaml new file mode 100644 index 0000000000..b6b4ec27d5 --- /dev/null +++ b/system/system_diagnostic_monitor/config/perception.yaml @@ -0,0 +1,7 @@ +nodes: + - name: /autoware/perception + type: and + list: + - type: diag + name: "topic_state_monitor_object_recognition_objects: perception_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/planning.yaml b/system/system_diagnostic_monitor/config/planning.yaml new file mode 100644 index 0000000000..c290591930 --- /dev/null +++ b/system/system_diagnostic_monitor/config/planning.yaml @@ -0,0 +1,15 @@ +nodes: + - name: /autoware/planning + type: if + cond: + type: diag + name: "component_state_diagnostics: route_state" + then: + type: and + list: + - type: diag + name: "topic_state_monitor_mission_planning_route: planning_topic_status" + hardware: topic_state_monitor + - type: diag + name: "topic_state_monitor_scenario_planning_trajectory: planning_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/system.yaml b/system/system_diagnostic_monitor/config/system.yaml new file mode 100644 index 0000000000..0377e68daa --- /dev/null +++ b/system/system_diagnostic_monitor/config/system.yaml @@ -0,0 +1,7 @@ +nodes: + - name: /autoware/system + type: and + list: + - type: diag + name: "topic_state_monitor_system_emergency_control_cmd: system_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/config/vehicle.yaml b/system/system_diagnostic_monitor/config/vehicle.yaml new file mode 100644 index 0000000000..4826c96e5f --- /dev/null +++ b/system/system_diagnostic_monitor/config/vehicle.yaml @@ -0,0 +1,10 @@ +nodes: + - name: /autoware/vehicle + type: and + list: + - type: diag + name: "topic_state_monitor_vehicle_status_velocity_status: vehicle_topic_status" + hardware: topic_state_monitor + - type: diag + name: "topic_state_monitor_vehicle_status_steering_status: vehicle_topic_status" + hardware: topic_state_monitor diff --git a/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml new file mode 100644 index 0000000000..a13b1dc9b7 --- /dev/null +++ b/system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/system/system_diagnostic_monitor/package.xml b/system/system_diagnostic_monitor/package.xml new file mode 100644 index 0000000000..d410e75000 --- /dev/null +++ b/system/system_diagnostic_monitor/package.xml @@ -0,0 +1,23 @@ + + + + system_diagnostic_monitor + 0.1.0 + The system_diagnostic_monitor package + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + system_diagnostic_graph + + autoware_adapi_v1_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/system_diagnostic_monitor/script/component_state_diagnostics.py b/system/system_diagnostic_monitor/script/component_state_diagnostics.py new file mode 100755 index 0000000000..8e12ed6656 --- /dev/null +++ b/system/system_diagnostic_monitor/script/component_state_diagnostics.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# Copyright 2023 The Autoware Contributors +# +# 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. + +from autoware_adapi_v1_msgs.msg import LocalizationInitializationState as LocalizationState +from autoware_adapi_v1_msgs.msg import RouteState +from diagnostic_msgs.msg import DiagnosticArray +from diagnostic_msgs.msg import DiagnosticStatus +import rclpy +import rclpy.node +import rclpy.qos + + +class ComponentStateDiagnostics(rclpy.node.Node): + def __init__(self): + super().__init__("component_state_diagnostics") + durable_qos = rclpy.qos.QoSProfile( + depth=1, + durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, + reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, + ) + + self.timer = self.create_timer(0.5, self.on_timer) + self.pub = self.create_publisher(DiagnosticArray, "/diagnostics", 1) + self.sub1 = self.create_subscription( + LocalizationState, + "/api/localization/initialization_state", + self.on_localization, + durable_qos, + ) + self.sub2 = self.create_subscription( + RouteState, "/api/routing/state", self.on_routing, durable_qos + ) + + self.diags = DiagnosticArray() + self.diags.status.append( + DiagnosticStatus( + level=DiagnosticStatus.STALE, name="component_state_diagnostics: localization_state" + ) + ) + self.diags.status.append( + DiagnosticStatus( + level=DiagnosticStatus.STALE, name="component_state_diagnostics: route_state" + ) + ) + + def on_timer(self): + self.diags.header.stamp = self.get_clock().now().to_msg() + self.pub.publish(self.diags) + + def on_localization(self, msg): + self.diags.status[0].level = ( + DiagnosticStatus.OK + if msg.state == LocalizationState.INITIALIZED + else DiagnosticStatus.ERROR + ) + + def on_routing(self, msg): + self.diags.status[1].level = ( + DiagnosticStatus.OK if msg.state != RouteState.UNSET else DiagnosticStatus.ERROR + ) + + +if __name__ == "__main__": + rclpy.init() + rclpy.spin(ComponentStateDiagnostics()) + rclpy.shutdown() diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/system_monitor/config/ntp_monitor.param.yaml index db54f70d1c..5f46821f98 100644 --- a/system/system_monitor/config/ntp_monitor.param.yaml +++ b/system/system_monitor/config/ntp_monitor.param.yaml @@ -3,3 +3,4 @@ server: ntp.nict.jp offset_warn: 0.1 offset_error: 5.0 + timeout: 5 # The chronyc execution timeout will trigger a warning when this timer expires. diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index f4ee2de666..db264d4b7e 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -53,6 +53,16 @@ class NTPMonitor : public rclcpp::Node void checkOffset( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief Timer callback to execute chronyc command + */ + void onTimer(); + + /** + * @brief Timeout callback function for executing chronyc + */ + void onTimeout(); + /** * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time @@ -71,6 +81,19 @@ class NTPMonitor : public rclcpp::Node float offset_warn_; //!< @brief NTP offset(sec) to generate warning float offset_error_; //!< @brief NTP offset(sec) to generate error + int timeout_; //!< @brief Timeout duration for executing chronyc + + rclcpp::TimerBase::SharedPtr timer_; //!< @brief Timer to execute chronyc command + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group + std::mutex mutex_; //!< @brief Mutex for output from chronyc command + std::string error_str_; //!< @brief Error string + std::string pipe2_err_str_; //!< @brief Error string regarding pipe2 function call + float offset_; //!< @brief Offset value of NTP time + std::map tracking_map_; //!< @brief Output of chronyc tracking + double elapsed_ms_; //!< @brief Execution time of chronyc command + rclcpp::TimerBase::SharedPtr timeout_timer_; //!< @brief Timeout for executing chronyc + std::mutex timeout_mutex_; //!< @brief Mutex regarding timeout for executing chronyc + bool timeout_expired_; //!< @brief Timeout for executing chronyc has expired or not /** * @brief NTP offset status messages diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 6ccf7a54b9..0b91b28b2b 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include "system_monitor/system_monitor_utility.hpp" +#include #include #include @@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) : Node("ntp_monitor", options), updater_(this), offset_warn_(declare_parameter("offset_warn", 0.1)), - offset_error_(declare_parameter("offset_error", 5.0)) + offset_error_(declare_parameter("offset_error", 5.0)), + timeout_(declare_parameter("timeout", 5)), + timeout_expired_(false) { + using namespace std::literals::chrono_literals; + gethostname(hostname_, sizeof(hostname_)); // Check if command exists @@ -47,18 +51,15 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); -} -void NTPMonitor::update() -{ - updater_.force_update(); + // Start timer to execute top command + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&NTPMonitor::onTimer, this), timer_callback_group_); } void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!chronyc_exists_) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add( @@ -70,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + double elapsed_ms; + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str = error_str_; + pipe2_err_str = pipe2_err_str_; + offset = offset_; + tracking_map = tracking_map_; + elapsed_ms = elapsed_ms_; + } + if (!pipe2_err_str.empty()) { stat.summary(DiagStatus::ERROR, "pipe2 error"); stat.add("pipe2", pipe2_err_str); @@ -94,10 +106,65 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = tracking_map.begin(); itr != tracking_map.end(); ++itr) { stat.add(itr->first, itr->second); } - stat.summary(level, offset_dict_.at(level)); - // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + // Check timeout has expired regarding executing chronyc + bool timeout_expired = false; + { + std::lock_guard lock(timeout_mutex_); + timeout_expired = timeout_expired_; + } + + if (!timeout_expired) { + stat.summary(level, offset_dict_.at(level)); + } else { + stat.summary(DiagStatus::WARN, "chronyc timeout expired"); + } + + stat.addf("execution time", "%f ms", elapsed_ms); +} + +void NTPMonitor::onTimer() +{ + // Start to measure elapsed time + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("execution_time"); + + std::string error_str; + std::string pipe2_err_str; + float offset = 0.0f; + std::map tracking_map; + + // Start timeout timer for executing chronyc + { + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = false; + } + timeout_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(timeout_), std::bind(&NTPMonitor::onTimeout, this)); + + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + + // Returning from chronyc, stop timeout timer + timeout_timer_->cancel(); + + const double elapsed_ms = stop_watch.toc("execution_time"); + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str_ = error_str; + pipe2_err_str_ = pipe2_err_str; + offset_ = offset; + tracking_map_ = tracking_map; + elapsed_ms_ = elapsed_ms; + } +} + +void NTPMonitor::onTimeout() +{ + RCLCPP_WARN(get_logger(), "Timeout occurred."); + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = true; } std::string NTPMonitor::executeChronyc( diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp index e2fc1b3a37..1d8ec80c46 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp @@ -25,7 +25,6 @@ #include "tier4_autoware_utils/ros/transform_listener.hpp" #include -#include #include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" @@ -44,6 +43,7 @@ #include "tier4_vehicle_msgs/msg/actuation_command_stamped.hpp" #include "tier4_vehicle_msgs/msg/actuation_status_stamped.hpp" #include "tier4_vehicle_msgs/srv/update_accel_brake_map.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include #include diff --git a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz index c430c21562..ffcb63f428 100644 --- a/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz +++ b/vehicle/accel_brake_map_calibrator/accel_brake_map_calibrator/rviz/occupancy.rviz @@ -17,10 +17,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel - - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel - Name: InitialPoseButtonPanel Preferences: PromptSaveOnExit: true Toolbars: @@ -133,8 +129,6 @@ Window Geometry: Height: 1862 Hide Left Dock: true Hide Right Dock: true - InitialPoseButtonPanel: - collapsed: false QMainWindow State: 000000ff00000000fd0000000400000000000005070000068cfc020000000efb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000068c000000c900fffffffb0000000a00560069006500770073000000030c000003bd000000a400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000005470000012b0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000682000000eb0000000000000000fb0000000a0049006d00610067006501000005d6000000f30000000000000000fb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000002c0049006e0069007400690061006c0050006f007300650042007500740074006f006e00500061006e0065006c000000068f000000de0000007700fffffffb0000000a0049006d00610067006500000002ef000003da0000000000000000000000010000015f000006fffc0200000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006fb0000005afc0100000002fb0000000800540069006d00650100000000000006fb000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006fb0000068c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false