From 17f1b956a397b1661471ad820cb53219aaeaaa54 Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Thu, 1 Dec 2022 16:19:27 +0900 Subject: [PATCH 01/13] chore(system_monitor): add maintainer (#2420) Signed-off-by: ito-san --- launch/tier4_system_launch/package.xml | 1 + system/system_monitor/package.xml | 1 + 2 files changed, 2 insertions(+) diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index beb2a4a782e22..c301c17137b9a 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -6,6 +6,7 @@ The tier4_system_launch package Kenji Miyake Fumihito Ito + asana17 Apache License 2.0 ament_cmake_auto diff --git a/system/system_monitor/package.xml b/system/system_monitor/package.xml index 01ef6d6f9a3a8..30a4af4af4926 100644 --- a/system/system_monitor/package.xml +++ b/system/system_monitor/package.xml @@ -5,6 +5,7 @@ 1.13.0 The system_monitor package Fumihito Ito + asana17 Apache License 2.0 ament_cmake_auto From 7aa93149fd79659ec16a62c5cca9734096dfae53 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 1 Dec 2022 17:53:52 +0900 Subject: [PATCH 02/13] fix(obstacle_avoidance_planner): add guard for failed optimized trajectory (#2398) * fix(obstacle_avoidance_planner): add guard for failed optimized trajectory Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * add error message Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../obstacle_avoidance_planner/src/mpt_optimizer.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp index 1a8e70f9136dd..56d74c9eab4ef 100644 --- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp +++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp @@ -296,6 +296,18 @@ boost::optional MPTOptimizer::getModelPredictiveTrajecto fixed_ref_points, non_fixed_ref_points, optimized_control_variables.get(), mpt_matrix, debug_data); + // NOTE: Sometimes optimization failed without failed status. + // Therefore, we have to check if optimization was solved correctly by the result. + constexpr double max_lateral_deviation = 3.0; + for (const double lateral_error : debug_data.lateral_errors) { + if (max_lateral_deviation < std::abs(lateral_error)) { + RCLCPP_ERROR( + rclcpp::get_logger("mpt_optimizer"), + "return boost::none since lateral deviation is too large."); + return boost::none; + } + } + auto full_optimized_ref_points = fixed_ref_points; full_optimized_ref_points.insert( full_optimized_ref_points.end(), non_fixed_ref_points.begin(), non_fixed_ref_points.end()); From 35b0dfbd78f5a3f36a4f41e2e45dd9fef4db54d2 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Thu, 1 Dec 2022 18:04:47 +0900 Subject: [PATCH 03/13] fix(behavior_path_planner): fix lane change path validation (#2421) * fix(behavior_path_planner): clip_path_for_goal Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): clip path for validation Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): use path point only to check lane departure Signed-off-by: Fumiya Watanabe * fix(behavior_path_planner): remove unnecessary change Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../lane_change/lane_change_module.hpp | 3 --- .../lane_change/lane_change_module.cpp | 19 +++++++++++++++---- .../src/scene_module/lane_change/util.cpp | 14 ++++++++++++-- 3 files changed, 27 insertions(+), 9 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 5f48501f325b5..39de619c4e3eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -20,7 +20,6 @@ #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp" #include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/turn_signal_decider.hpp" -#include "lane_departure_checker/lane_departure_checker.hpp" #include @@ -44,7 +43,6 @@ namespace behavior_path_planner using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using lane_departure_checker::LaneDepartureChecker; using marker_utils::CollisionCheckDebug; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -105,7 +103,6 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeStatus status_; PathShifter path_shifter_; mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; - LaneDepartureChecker lane_departure_checker_; double lane_change_lane_length_{200.0}; double check_distance_{100.0}; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 4c6cd3a2ca018..17199c68fd895 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -50,7 +50,6 @@ LaneChangeModule::LaneChangeModule( uuid_right_{generateUUID()} { steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); - lane_departure_checker_.setVehicleInfo(vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()); } BehaviorModuleOutput LaneChangeModule::run() @@ -146,6 +145,7 @@ BehaviorModuleOutput LaneChangeModule::plan() { constexpr double resample_interval{1.0}; auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + if (!isValidPath(path)) { status_.is_safe = false; return BehaviorModuleOutput{}; @@ -387,9 +387,20 @@ bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const drivable_lanes, parameters_->drivable_area_left_bound_offset, parameters_->drivable_area_right_bound_offset); const auto lanelets = util::transformToLanelets(expanded_lanes); - if (lane_departure_checker_.checkPathWillLeaveLane(lanelets, path)) { - RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); - return false; + + // check path points are in any lanelets + for (const auto & point : path.points) { + bool is_in_lanelet = false; + for (const auto & lanelet : lanelets) { + if (lanelet::utils::isInLanelet(point.point.pose, lanelet)) { + is_in_lanelet = true; + break; + } + } + if (!is_in_lanelet) { + RCLCPP_WARN_STREAM_THROTTLE(getLogger(), *clock_, 1000, "path is out of lanes"); + return false; + } } // check relative angle diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 7e3f55513e22e..9b3e65ec92a87 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -496,7 +496,12 @@ PathWithLaneId getReferencePathFromTargetLane( const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); const double & s_start = lane_change_start_arc_position.length; - const double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; + double s_end = s_start + prepare_distance + lane_changing_distance + forward_path_length; + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length); + } return route_handler.getCenterLinePath(target_lanes, s_start, s_end); } @@ -511,7 +516,12 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, in_target_end_pose); const double & s_start = lane_change_start_arc_position.length; - const double & s_end = lane_change_end_arc_position.length; + double s_end = lane_change_end_arc_position.length; + if (route_handler.isInGoalRouteSection(target_lanes.back())) { + const auto goal_arc_coordinates = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); + s_end = std::min(s_end, goal_arc_coordinates.length); + } return route_handler.getCenterLinePath(target_lanes, s_start, s_end); } From 00fdc47af4105d037de6144db6670e40bb212a2f Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 1 Dec 2022 21:17:15 +0900 Subject: [PATCH 04/13] feat(tier4_planning_rviz_plugin): add offset from baselink param (#2384) Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../include/trajectory_footprint/display.hpp | 1 + .../src/trajectory_footprint/display.cpp | 9 +++++++-- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp index 540c590324810..d3964f9e7ce07 100644 --- a/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp +++ b/common/tier4_planning_rviz_plugin/include/trajectory_footprint/display.hpp @@ -67,6 +67,7 @@ private Q_SLOTS: rviz_common::properties::FloatProperty * property_vehicle_length_; rviz_common::properties::FloatProperty * property_vehicle_width_; rviz_common::properties::FloatProperty * property_rear_overhang_; + rviz_common::properties::FloatProperty * property_offset_; Ogre::ManualObject * trajectory_point_manual_object_; rviz_common::properties::BoolProperty * property_trajectory_point_view_; diff --git a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp index 7ab59c239f01c..b9c32aea58be3 100644 --- a/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp +++ b/common/tier4_planning_rviz_plugin/src/trajectory_footprint/display.cpp @@ -44,6 +44,9 @@ AutowareTrajectoryFootprintDisplay::AutowareTrajectoryFootprintDisplay() property_rear_overhang_ = new rviz_common::properties::FloatProperty( "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), this); + property_offset_ = new rviz_common::properties::FloatProperty( + "Offset from BaseLink", 0.0, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); property_vehicle_length_->setMin(0.0); property_vehicle_width_->setMin(0.0); property_rear_overhang_->setMin(0.0); @@ -158,6 +161,8 @@ void AutowareTrajectoryFootprintDisplay::processMessage( trajectory_point_manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_TRIANGLE_LIST); + const float offset_from_baselink = property_offset_->getFloat(); + for (size_t point_idx = 0; point_idx < msg_ptr->points.size(); point_idx++) { const auto & path_point = msg_ptr->points.at(point_idx); /* @@ -169,8 +174,8 @@ void AutowareTrajectoryFootprintDisplay::processMessage( color.a = property_trajectory_footprint_alpha_->getFloat(); const auto info = vehicle_footprint_info_; - const float top = info->length - info->rear_overhang; - const float bottom = -info->rear_overhang; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; const float left = -info->width / 2.0; const float right = info->width / 2.0; From 96652f9682ae7a4e10c8f5840aaf375677528af6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 1 Dec 2022 21:18:22 +0900 Subject: [PATCH 05/13] feat(tier4_localization_rviz_plugin): add pose history footprint (#2387) * feat(tier4_localization_rviz_plugin): add pose history footprint Signed-off-by: Takamasa Horibe * remove unused variables Signed-off-by: Takamasa Horibe * add maintainer Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../CMakeLists.txt | 2 + .../icons/classes/PoseHistoryFootprint.png | Bin 0 -> 18815 bytes .../package.xml | 3 + .../plugins/plugin_description.xml | 6 +- .../src/pose_history_footprint/display.cpp | 253 ++++++++++++++++++ .../src/pose_history_footprint/display.hpp | 108 ++++++++ 6 files changed, 371 insertions(+), 1 deletion(-) create mode 100644 common/tier4_localization_rviz_plugin/icons/classes/PoseHistoryFootprint.png create mode 100644 common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp create mode 100644 common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp diff --git a/common/tier4_localization_rviz_plugin/CMakeLists.txt b/common/tier4_localization_rviz_plugin/CMakeLists.txt index cd416012a21ab..847c5e8af5fbc 100644 --- a/common/tier4_localization_rviz_plugin/CMakeLists.txt +++ b/common/tier4_localization_rviz_plugin/CMakeLists.txt @@ -13,6 +13,8 @@ add_definitions(-DQT_NO_KEYWORDS) ament_auto_add_library(${PROJECT_NAME} SHARED src/pose_history/pose_history_display.hpp src/pose_history/pose_history_display.cpp + src/pose_history_footprint/display.hpp + src/pose_history_footprint/display.cpp ) target_link_libraries(${PROJECT_NAME} diff --git a/common/tier4_localization_rviz_plugin/icons/classes/PoseHistoryFootprint.png b/common/tier4_localization_rviz_plugin/icons/classes/PoseHistoryFootprint.png new file mode 100644 index 0000000000000000000000000000000000000000..6a67573717ae18b6008e00077defb27256415663 GIT binary patch literal 18815 zcmc#)ig}7!VL~0BI3P>4u>jX{5WQyQGE==@gXiZcsv`yJ3)ShVHnpzyIKV zx%UIl!@PCQ*=Oy&*IwtGaAid)9L!gkAP@*gMjEOL0wKly`$K;YTnXsp8Ulf2BxRtY zY95&f%V-`irs2ZJD-E_>W|QJ#$f(lDGUzn9FhfdJ6ls+f7g13tl1h)8=nGVpD&-eU zEXJbkUMvxiN-bt%sdy?0Ws#;2U}Tww$D9k?tJrv3b90N4w6q7mY>Vu>j)cq`Gq>&g ztxVotd7x=_%0QrfCTU6#h!Tqc9R&LG1r-?ts`)>?Y`h5pRS_lWag8OXawfXJHk9@r zwSCxzbPRm-g9n1hq$!Q)8a9)#!i(M_cz&h|6vnpsyKVD7twGSL0-4KL6Q=BD1ZeP- zzfzYzWVM-q+?d)>KwJl;5;rOeaVP-p8?mb$R~c z!r4STVAB1bJ?W`Q1N^wk6a>;QVI%JxOu}Vf{^G|)Co3MGH{(9RMV<;nHU4Za)%KSC z`;I=`QN*K~)?CjlT%q3CJ0d$G%J_7^;$JI4Ms3f{6oyJDbjK_N!4ju2 z&$?3P$rm$WTureE-aq`r%H%qf&xx}1+^E65XE7*U*@ASHv#;8ex#{Xeuo?I&+x*og zCp;~E10-qE582|U!y4K~`SF#B31Gjpj3?LDSx6hf<#=NRr5x>o;P$^l9osYrEfL{WYjQydU9;g?K5#3k=W*FphTCuCHewl5&`Uu9F)(f>MMOxB>pE z88huBN|GrNq|eH~M(ELpl-tdZkMhs%;OV+^2?6{7^3=m>h4(Sn1AF4SLTeDL;O9g! zBWaM6c%X0DechlP8B_BUQ^1_B5ImG0q$&AGba@Q1!Sx#uv;b)FNlKJ%FF!gu4goAi z+jugMiX+lt3%Db%$x}1fa)yh%A&xxGW-Lle9BaRZcr(Jl8YJvmXVoM8b*d?nDn zr>xqjT|4gjOmgjyb!-I_L=1WSO(};jF5)F9(KK9k$6juv0YKyrQRHz_DK~8jI2)+w zm-Yt$QfvYLhOutzxp@d+SUG4qO=vJ%C2^D=7atD;N#3;^c;DQzmR&&!oPfkcBJT}x-4FY#kT02m@DxoqS-{xT|$ z3PAj4Y06h5S$bR{FjZhmf2ImPCOB3Cm{OLqda0jhqvfLSxmZ+?A2<^h+z64sYjMk*OaM^512bTNz1yJX#3oL;p>F~$69vB)J^K&J0kyCuHxo2qc9?N7 zpx^)Vh!`XhEMW3-8hQ#=jbwx3L_Yk3cspR5f1h~;US}c=&En=n$AFqJRa-Trw&}AF z#AgA`>wx2@JWAg4l9l5}1GY|0Z9CNsXNdgw(z**Vj@S4BOp0I}AA5Z|Zl~>Z3kVbj zqQ+>~?`j3P7S~|HAAK#cX8I@4)zn@Yx%DiDRJ@~c4bTC4ySPXJL&So)v8RA@6#{J%M_y$DCRzdd0*FPR zEH--gPG5+|{w5Yegr~;hMSkzb)vYj91mGV5xcRrf*}fIu`8(Gk(oJHQTMWSTlpe3W z7U|f@97Ozui1Pqn>xtK!34&Gp1$<#-lg>4RGs7K!6%!+v&|>f%Vhw^400u&sGY~q5 zJ^R2hK)+x%+?N_ujGMppd5=D@{V_pS_0HC2*7x*eApoKv>S`$MhIW>f zMXBEn^5P16FfqDGOxdOTX$;a&7%;G-28$P)$1&8Il!2^?K;(-9A|F432ZYVYM^|}I z3>OV1q`?mb9wc7}dJzwJ)_0FSe8o7Cm(><8hzS0{vBcWu7VZ9!xVd$!*%0qZqPEJb=%hJ$Z_L zz~Miu9bQOq9)39Df;+w>C4k8SnA^IR``!NH4|SUf61t|;@Y}Cs7L7w2kT8`ufki#> z(|*WcvwwZbRQK{xx%^U^9it@ui(i`PZ(a!{U!?$lf%er!QwQi?Jq@BTnXsQ0^KiZy z)@H)?s!l`=|Lg4jvo;>IFs7CP;9l8V4UpAo>sjIq#`%v=kuRv~Dh zsiW)pz26t!@MhZgf>?_yj35jsQu16~;-8_l{?rj9%$d(V-9L6Mv^t{|GGZ5-C`W&WqNh29e6d|hsvmvzE$*g zQmyMXe|TD_9?2}u%kdXf_x#(4xd=B4sAmzB{R*=+ZDMv*{0=ZZw`U z1zHnRLzGA%_kVvk=&UnLH`*nK6OLz)IZ+1RaP z$_a6)pzGO}t0_(S2EeN@?u0pWGf^QOT;Fy&fIf+061Vv&k6#eQ9E?CJbN7 zR~KK=eS#kf?q9C)BZqF)@T7l`iF(-E2|R4|?91b}4Ez)Kk2FyYsGq$iY-#*_u@~1} zergZr={EWSme9lc1Lvw@G~X6iq*U zJ>6TJJ1l)Wu8Wk+%I_jO@X>ttg#2Gtvr6H&5?wENhU3wfOD|1HK$EE0JWPIo?o%@w7QX>1z=lRIcDYmm^RdaH!J$U0qdKjR{v?Nj@k8=mLg z!QaffX+{G^hFLx>yzv{!{4I7+*?C}Mw;W;Akw&+^oGNB zKbo1D;RxOCe8*HY+Wq2Z{a&$bq>kL0E*5uqV%bZU97(Rgg}axR>R3#V{3&SGo$cMU zo>il+6}(CD_7>Sn+RG>LD*ZH>q}A^P_|jLN;2-ZSZ)e^-CR7g&UCtF)C)#ml2sjLp zna0G_?3W<_14PCTp7u%y`1jJLdPCb;WW==}ZA1fH$8?tCsR>bJF?vJ$I|J{? zlTYZ~d#P!IC7nc5MI zRsbBkP4l%D7PaBB{&6DKt4yR^NwBqPM*lr^I~U=AH?+LJ*&C$ZAXr^~8P$6rvc|OQ z@5*T^JLt$TP5T0t(=wiG^sXg zEd~AVuZVrio-s!T&LrqYTve>D*P81_=>7;qF3oJb%9-_$ zl7tF5{GK`$Y)Po@y_Df6U|SUSg7!xbv_*4yVe!Q#Ebc;ZcwoSK)rHUB2ho1zr!2(h z%dVk7oadz}SQ849EKz@)p|3b8py_+bZq+KNZ|#0KdDLC_ZhWXy#9XXyyOh3&D_KBd$d5r~dZ&@?gHCJhG5mniAQ@R>y{O zFvmlJ?JS~a-%K~9k;T?h@u&=-btl<6W(Mr?#p_#Nvx_0v`ilrR<$^+%&Dej}o4vNPn* z;owRb6rB0y*XFvkd6OhCQqGB6I?r)a!X@j6i&CJJ1K0a?>M~fR$7l@HLqkBtR{DNi z#>_e~=e6CHkiV3$L3^TB!NYiWD6Xr+?LjRJ218fc9?44c9VrfHEASeRf4u5=;4Pz2 zbmAarlXGC+<}+MNXuEB2%{j_j84W=YGt*J6aB$pdMP%f^yA?e@n6EWm{~fLLbbmH# zB#6dX=5Pv<(zsg%*jkAKL5r0knZM%tivqc1CatP(x82cXPdwWwcmjs-W@q|ImYY99 zYwlE~Y-hze~lk`x3mjPB!27mzk`xg^y;8^_L_ahVv6~)|t~mfqeU``w(TZ zxrTjOPssfK`oySR$6>Am1>m)tK+)!_P=0!6FfqlwTf!xubMJn*u$lh(uwJ8c<*Bgn zDIx;XIk9`S^Zs-&ZN{2BW%h21L^x#Hy{t&3#$u%2EIrNNs9uMGjS4;G4WkVIc79S?9<_)}5sGh^o2GGu01pfp6?z7wr_`sf6M}tS@hT zNCXk5MDOIEc)b#>BL5Byp4r;f?uV$qGqs%c8FpR1U>>e-v`u`iQ65*US#Q)pH|_z0GX?kiG%%(LOU!%Ckq@v`f@JK|pfX2{xiK)~<5soB5N zZ^n6hgiX)Z0q$xxBO+$zshAz%JT;G?R^ui!yaQF=CYolEefA_D418x5EBtuv6S_T# zt>8C5ZK^P2{Tk3jP{74C9i5Z*XB90Em|L^1Lp$Ty?ZVFx%zQ38_!1Hlo_wx*Jxm>5 z;cSTnM62^vnW0! zv(<%*u%dQ#ZEdDm`^UPysNb89-plAwdG@K0)y_jBD-qUQUN%)_5(TsN78%{K)f*=5FXVTI-5jC-M0r)_e2{Uu>Io;LFZ)WTSayCh zH$y%7yq?2_WZ>t-lJJ(ErM5;8)CMsVjU&indTZFQ!V%ji9)yN-+O%Xp_zMN?x2c&~ z$h26bQIZIOY$0|Hl-A{A>HW>D%Vl99Us^*}%6q+6(o_lL&Dq|Mo_&23{zG+CyVGoa zZdPuB-3?-rE;hgTQB^{1j%hnpE81vUZxe{q7+r62)9R zQjuMBfm(#Ms?@4Z$;Ik^iw9oWqRW`6isW|}$3~+7uuRp_uSRo5+uoV%o;%fYOqe&( z|2-;yrIE5RJ-g8wo6hVoIj1wlF#nl)eXp0d4XYB-b^eG|Cv?<)1s7iS_r5%WWTvIf z2b#SW2vI-|e@Dud15=%5ScGn5xI-4JpMXeprsukuG_&3pJ9vw|;{D{iSX$ZsyN-QS zIkazCCe z5tjTIqBD|W;A`f4 z(o2%2vBi}U($ZixbedSg2KMZ`WSTOQSc*AmMXpEHQN>d_Ts3UpJGs9*!n#G=T_Pyr zRXRI_^rJ$49%InZRr>IOdrQ?H&qWVYSv0t<)_MjA=@l6!>}m{uAv-nCbu4CkNL1I0 zszx2HgbeMhEPPFWT{;-iq%i3PmyXoFdjD&`g5k23{NZw;w9<>q|Iu^oZgl-bE6dSw z^mlKsUR{#$i%`XX%3&%n7Tqbvr-=iv3Vn!kYG;%8(aknQHmT914|hK(l9+ATj)ON8U+A7k}qtQoh z(o1n!2r2de-qqvX8LPa^T2)TVwotK{{5PUlaF}h}t$?CyJIDXX_j2AsLij`a@Rsi6 zT9`WmSJ@(5~J3(YDA@J|o3An#c=nA#EWWa`laHX*T$SjobapA&u?(UWP+Q{C%@7Q}QA^ zJNv{`cm#_$-9Pc`0QTcIA;7WaQ;?NPKQ)wA2wg2XAF*`a%1+DL{Ei|8qxs#0>_+qE zIygdQ6ZJGXrnY0=Ug)~OeO>jx(RvJib8E2<`5+hRj<=UQ`XSK&;bJyBjKUK-vv>=H zGCKa(@!Sj)ME<{YC-0A1b(Bqx63NF8LaQ+pwr7++?Y;83W-@6>tv_S?kk#VM|MVs2LY-DySvyPfa--K5g8|I^)y zxR)i5=c%cxpXe{y&Af@{;>*~{b02i&{aB_$54p;>&#@87qjme%iw$;g@1y3hdBV!k zZH`JGW7Tng^Ak~%}#;Q-2Kv@J$5 zPoiIQ%EmkU-JkTeJJ;0SF{mVKykranq(ebDsHb&e)<5FL`@L~x6=ryQdf$-7vCQ0< zVKw1(9v>zMv~Hr4MzS?k%k=W&z-eNFmOLHeG{ZZM>d@B7MqQoA_xwa^NGPaDE9_=G zqs6AdhWYsONzd@+OaTr-8XOBq(wvBuPJ?Raqs7jrj-Q(vB?55luOh~nXxNd`{VH~I zM;8yWZJr7r(}dDk^xNhxwsalXR!sA1N(T6xx8%3 z9i?;He4?%(U&RqXs^zm7y_}z_XZ`rKJWdqi+!lDPbqjFCc03mCyq$|H4*?12d18Xu zsc9-IL6-9k#5yU9z|37?M|bFc*A_tKQa1XE*M2$dVt-b%RtQm5BcL!+`EtBIz{=SB z>EVhR7s6-iy&6K2n(c0w;sbP}fDO(N@z^>W*1LXDq9TP$lSj^&IGFi{KpGIP-++mV zR2tI$TwO=$)6FcJ+Rs%5-rd7wzS5;BWefCwh}pkOiJM7&g&1R6o^Ft!tBsfjK$~LV zchl!;(BZ>dy(Cy;~wVJ*0yFzq6!EcXR5$rY?4gI54mUi2JbS2$QJGSQ5Xyxt;M>AF4Sv@28`bN@} zlqwK4bFtGi`_*QHwN9+3#N)ub$|i36r9T2zdP63$?>Q-i9luzG2$gB(w!3~+Q&2P3 ztg~7hwvPLAu**HR*1BKO`siW+cbQb!0_qL(aGzg2%;p*MKf-C??dsxN<>Sk(?%vrz zzc_PV+EeGDFeZ;i84*ZtW=H3=#2j|W^|ND<(N6Ys^vYL`H`p!um`KLX43QJ|#XvZ& z5VNDI>gv4*nZ&heTY8$rMpSG1;mfQ=IFry0%W*2X;r!dHgLxC(0(fUSoX7 zwNjEXD0t!s_`m5$=5BFW$;8yBAcYLBacVr_p>vrk2KwkD((ynWWJf^m6M0>Rs^j&K zcuG{Bb;k~FsXL?edF#}7S zHZDLmq@^Y_Y4fI*ecn|MwalQipJKC!`&+AEi=Oxumg^tYvS0s6v}&dED>#4Eap zAJNq9yf;-G>9>swxt+fKLmkVyKVy>_Z%8^@94F5{sE|(%yu=RF} zU|f+?>1X;0!Jp9#;p0r9dszlE)NBFz%Xb>g4B!|F;iIe%Rte_9u|W9MNnz3b-6obM zRP{c^sD1ZD1@8DThqDat@NK>7r_aG&(BpD|3!JTXho#kHn#b%IMn={-%208He(I{j z8s5K{sLB<8*d9?YNvdlAkE1Xq4M&^5V`}zED!eajyno=M;-hVhny2RT;+R}Z9r4hs zHg0|z%N0-eaMaHz%=Q`>bdt9ENtBg7zrZD9*0FRwt#|ss^{%z@XNh|0y-*Iz&V$F+ z5b1tlj^6+i1CSkW24b4a6t9GRb-61JetIWG3TakcE8lU{iJ1*WfY zhT3M?F_AEMwx8^2o>J9?B73d2M-Ao0yblIV=w|pGk7+ zChG0Dy~rHOK1Z}(>0iZDOF^?7HV2aYQZklSUv5SgTzl!7E#o4p(rOC24ScRx|^VD=~3JdRynp4w?)W(5A0 z)_C3;5w{#ut;AeK9af+D3$6?#Btje>?{BQL*L-vD2I8Ik7Vg!@QnLe zppa1pgsr!=`wq1_mTHZ}C3xl@K9Vf_gQQRAD_36GdaZ87H=x1+REu&}7i5gjPv`MP z?=NN3^VvFySA4Is?wUDmnv~`eOyF%d=CNUipe$ZOJ}FZgXz3+QW*Yt>xpMko-JL05 zks6_=q<8Zb!SRc%GSdhvd#^TbuH?h6Nef0sA3Wjvs`Q+zGw6AI#HO>wHLBBX;iyJI zFZ5A$b~Ji_KEX=v=l0q7@oKkklDfM5JEnZw<(BE1>)c*>-8TRsJgfKBDNYu#C!HGU zbLvbP6<#?=;4iU0T&Pb*LBnMo{A)%V&E)8oVNDoVfNMGIGqDC~Tscy8s*HW5h(C^+ zG%$nRO&>=ccGarn_c)}SF|TH0#$7Cx8+pN%20+veAhSU?RF1MOcSo&5t!{_L=^j!S zwpKQodunYpRvLi#(OCA4LtJoKXSCHK;j8k9qyr5?e~KQ|t>1+SXe) z>)&;E*Mlsb=gfL7YJkKP1nqI#dXEOw6i8@(OKrTvK3jR+J9~Xn-iY+EwA~bLWlsRR z23NE2O*dU)=1+e<1VW`GQj_6h3MbAnyDHL5QY*f|gl9W++|d2#xN2PS<3Ii9G|AA= zOQyQ5g5I}YLVzr($N9aHm+H&mo*)(pv+|j@cP5Nnz1nAZ%$JsKX2hLXNhUIS$$QnG z1AL<~%s^invTXLbb=gI`FD6^i`_j_7{ay3FG_nui=Feq_SET$RM11j`*GRQ*M5-@2 zd2E<{+liz^%K)8H_K-`#W19`%J018J8$M5t{XUL%67!Kv?T5Y6%HoSPfj9xo@SRNA zO&i%cnz)EbdPRxH)73v%VP9f)rk1ApgoAeZVYB+))9p8Bqd4&bBR~*`oNY>09O~N2 z!Rp>7KnHX=U8>;-697~ZJ_sN<+Irs|Hl_`;G1z694OixT`*V|Cz_YBGLc(tM@Kohm z>JD&MYd3Xf+B|B;lk$xwEPD9_~%=V(vx(C@a6Y-H$#=bfVOn57$&>rkwrVs4#bi zIXh7e(IM6w&y9tw+~I(;y6HV5=yM_(oXxC~r9=|;e>##2d}?n{dgmZtm$kdn2_X2$ zZn1&1c*eQwvMIIRLX^A-fxxJ(8WBPe?||LSuy z{t{iR`A!>-41uqe*4_8|FWHQLq|ffyeq*aJzDYNo!z||t!@KwR;k$ICGOlv)`wp!Q zFe7=&V)-C1g>f{5f5E>X@5^pc*;|x_)H3t0Wj(Hdf}t6f7dj;Lm3y|b61$Fgis3HI zZZTE=iadp@o=Y|={({-`cL3Mp!SitW{cQ0t++xA?d#mq<7jHsy-_FH#^te&BG6e9=TM4K?yE4$gLtJB{QzeetB zryUa6*x1vKi=Ur?^*FjI-@8jse%C$IlbLGM)=y!06b)*c>gqrMkCt)IoHxxwf;h>K zI$ZxUI0Qd&(@HM3+nD1SE<}@A-dyZ1FM9v|iBvCi@E-ah@k7-!4x7-5u`g+?MqO*l zm)P-Zzu$iR6iAlY+w9>D6kT>tx3it8Zf2XNw7OwQn=-dqz_;&l(OUI~)JrvPrtJfn zwVGJjkAIYJ@iHeSyf)eOw~G0ZJld2UGPIIZu3h{4455%Qbz6lZW74FexmYeYM;8aI z$K-h3Y^GFFCuw>-YX)_9?Ig^9A4wx1xQ2#eYVG!-LK}|bv$R8@eqlJgC_fD*}q9KhIE1f_i#O7_ zT)%x`)T#K2OJP6!v6Vb?t~cc+MQq6?;as!glC6mA<(e z<%lFRX`h0@1C67z0NHWh|ZjuVZ|V8t@FP@kIUYdtPp)2I9s3B_BXeBp}-ZP z5|l~AJ-|-~e7adHXKm6rZHQ~nBqb!qRwUhuE_?tsh^Kl=Hlz{jJv5 z%a`iA?S8PdtMf0~>M^6t2;7z3^z?rn(s|R}N+Jo!JO1-*rxQyqqw0F6X>RM!ACLOT zz2%2LmeJ%LT4#%}#EGCHI_{2UWy@EDyhONJs|(RvT?Z?St5 zJe~uJKn9oH8ehO#Qs>h$z{xbsOPOK-DR@vG_GmD%r#DqpIINVFeAg~=yeiV@S@ycUgHP|m(|5vgek*ZEH4RdwK{d$6jL>+4X z`4ChMuWjS9nJF{!ij95ygn<6)v21l;z0gZ@UjZab`9s9KK++(ZU}hS#jlPp@YHsdX+N+j<%}bAm_k9)T8@LAgyH4JzSyw>)>XO5eYZ-f(KZq4CZ8KAE}&p33a#nAw#ZL!zhkYZa0_fy&Jh12UfTAj4W?< z)O2(X-CC!M1+7n}8-n0j-m&cbyUhc#29lDYkyrH{#~wfcNz*@%?SyYHR3jK#qYkg` zfOxt;Xp>!NetI3?-j2$*c6E0peFJ1k0qtRyN~0J}eb0QIcI)6yPMaCaqc?e?=5%L0 z!wWTLgHwtNdLFYf->C5f&&TSGJKKO9egTz|q7H=dL_|bSuP9CLa~>76 znCtPqJkWLkR->ldp7iOcx({hE83#s3x$hnxA+ElF{8rgnRi*qnM5H$vfR4=(~l-hR-WQi)59iT6|TZNDzuRd5R$FCqNF)uguYRz#H!g{_d46@8|;6d)C31g+?wwl9UWUN!_7@iU(y8@`IP%@=plBH%=02yJ&ynGhwto!JlUOX z4ZlnR=UHTX(m73bJ`MY1G35^fepmv;GCh*P+1Gq7e{QeWW1q~jSlc~K9=z&1x!R5Sw+>Pg|4d7B*QMP1eov^xZ{boH28L>~l{Zhb#6){LZ zm@6K(L>wtG?EU!}!s|J9nZo{zoX5(&q`-6B*F~kgn1RWbz(S1+KjtWyba%eqCgn)S zFN@z*^(@_A3M$`75z6`%5E_kQVPm~8vUW(0-)Xpgi@MOw2BBVq0JDHsQp!G~mGIp` zt+g8jO^5Tn$1Dr}bH1cx#Vy}6vXegp0~5m=T_@{(WO?uY<)bNy2_cW;Z}e~9$_)RA z&R+=3N)8DzIPSQK_PD=ZR~&XP^$^GwPJke6@_WV`6v&1R5CO#>3rm*Q23Pe8MP7zS zN4FFUff1Z$-rRg+ zUOSJkl&izPHIn6>?4GZlL7(#}f01PSSIocavf%MkVJQEWW_pySvMFl!k06}^LD&vwY0rFM7Sdo{5 z!^6JOK>wyp&Wqx=>r``x(>dQU>14zTUKY_wnmZX{if`ugWtEtf5;FbfaR!RPMaJd=iiHkN)+{o zw=7_sEj@pbvWiNnS(}3oHAP5B$P(ALz1hl~T>aEY-XZhaZD8r8xs7nX)xEONDCjv; zOUTNmo&)Iqji8`liKzLOKAgKFJ{IZWuJ!WaoUryyAsQa}xqK??8(ZCEx7KBV%c+g} zKb}#TJ3DI#oj_gNy)Lv4->#&&w<8@3lV0qv{JbmRz6E=*>$kXAjlB;|IzAm_X;uCB zv75{DlZUCPX_L_PUzBOWE8nZ-zMA|iD#R`FU!}FRHDgRnO#Q#U-C^A-YHAzgg4T%^ zfT5UUrP9zgG!1;~y?UlKy%*}N2#Hw+^T|{6+|stQd7Vod9abu&XJ-I|3zTY{NPshIIhMP4(e)?%0Uv@SDj|pppr_Jb?SU%^oes09s*d=hLG% ze`5g7Pg|}#S=k6d+Fsg@%%xAxPEJlx7nk#=$7MiN?f*(I2$fs2BE8qna6zlDpw#AvWn%g%t!02^>Yj~m`Jc$>iH%XJ+e zZKMxoZxEm;V0$ox2?Us2H~)s3I>H<)HIzL9C2*B2%2% zuoz$qGG)~AC_jk0_89l)-w}Kjrep=sU*p1KA%bo<^)Jj!7x@I^8msO> z4>V)boi5cC6{?m%cYKi1zpQXO7Sj9MjcR~0ZwA;qj*h|~Zix)e+JeBwbFm_`UgUBj zgZ3CD_k(a;jH;dGQ*p3Bd%O`FzipXyFXu?vx-t zRRt(cFcEnGVV%fzDOH|TTM%e0EhS#yS{w_?&t<&+7NqhCIwh;+O2Q)sTsc~YfC3bU zq)x}416#3Ue7ioXI|D~PEaz~kUWr15fPZx;b=F9Tzwr0+<&|tGXq@ z-!p+0U!6Y(5C2L=UkFUuSvc=t6~zQS+H_VW)Q7oK9zW$`$z@cCY7+mxgHfQ2uQhZ-Bg6F)mmEz|j=0p4I}11k?as_b+mA%#j1I8iXEwQXk2s&yQGt+}5bJPGb zYs;@L`=D=*mVrBcjz(@?+%AG&0^6ZbXHPVwwQtYzgjukZEJ2bVC7?Lfli&RQ%4ql> zZ3Bjstq?yQBdHTP-^B*^9^)d5i0V_-08a5k6bqU|cZ>@XF)H;$15M;U%QGN@6}Z0a%$-dB3&%`n1N7*~lrtIJ9})K!Z&`SKbd?mA;INup)SGh_}|FuM4}K3U^9Fb8+f3I)=dOLQaPRBqE3YvwXIT1%t;9*xKkQh>Kn}+ z;Q<4<$i;f+^O`#E1n{wLZ`^)rpKs3y{DkQ8RNlH1vu<1qB`m4Ar|D>VgG6BuM+f>~Dm_|t=53>L+p^5?Ap9utF zppJ$KnZyZs@R1KmMUXW&i^`seS3FLP#h)k*_YGfPxP+Lf^-x z_sPQj+kVNDcrI(6;l2Q!R|cD$^7tp7bf7;!t@&f~h=LG73c#aHVNd&EsO|JDJ9OOx z4GDygnsW{dr3{vF5xpd#k62wqCZIlHJHXZfU5XlU_yV<>Rk0;H^GfkD(N zC8;^cd!U6lfJ^KX0@2Cm4 zd=8d)ev_63t*c7*w-JRN1{3v+Dme+gIiEr$x928QNy2pGAlduak_Nks{AB%r4d$fH zGhrW1qRbq8d%BIF&*L}E-fw%oD&cP&m%}Rqb%t5mz_v?@_2O7|JOgdKNWi}S@&06z zP{QC}7)~36DkZQSelp`cI3;!}dXmDmA1C4Sk2IJkXca2?Rq1CeU5pr=Z$Eb|MA79r zy57rE7PN*8KqGRYA39>(eT2c|(NGsfr{%mU54|>P>rf=dU$a zVl0M^67U>rcEgp*DN>+P_xfhO3&BA|ja^IR9nZ)3c|`h+cLeVW#xuq+3uv}nG%A6$ z#JM=Sf#pn?8f_ORN$Xqfs`b>*IN!zY+eSWe1i~INHf-G~8xDL(&1R?cv=sOgWv}k5 zvteF@Dw-i34yuxg_Ahk2WZ_t?;YIDiHl87$aT0m1go-04L)r0u>_1x^n=umxKkx@T zXKB)YVqzl*IYMPkmI~jo$xVn-{?=xiJ8`YE6AE&DmVLZJomVQhqu;I!nEhjZSW}A& z9aUJDnK_|~5?EL>2L>l;*|=qJIbzC{IPFmO%Mz2JRMpR3A?xzA_EP8?g@56F6FM$q zslHsKdNUki-|0=HpNsVPE(Hds%j;8TT#?q*MZ~(-<~C%b_A2|X}`fHdwvLeljj~n5@a2L|D4uGt&g#XQX&30uL+(&sRQi{`=)W|ELz*%Z{nK;cEHSxzI zsrA`<4Rz6Z8InRbWPpDMuP7)CHtM(&gT2jMVmcA6EvW4pd-}Qk^8M1f ztkSjGqJ~jOXiyi^+0T{nVA#S8`(S%RprmWLq)G}RWd2UQ3q_lH)oTEP+*uVvg!LsN zp4D^TqM1J4uq;@HeYBs!V3ZI;JIi2~d7$Bqe6W{zYP=5{*J{>)N}3r95vhNZ5{>PP zuI(%1IF+xoU4$(JU!{WLTR0_#*HMh#ml-ls=i#wjLt?A2l@z+gU?w4~g&W3HA^s`4 z4F}UB7IHlO-A+nWfeRdy@gjRdiHelQl{X=Mi%^&FKd0*<@l5nI+3Vc#h{rC$Kmoe- zvpZ3bMn*|-qZj5396y5q;CzC9kn7s`b9To2s502|i^nLOu-LIIk#*-sAS;X19xv|T z{0*ls}liCZmq|f?Z;8zxgGkM?Er0=gxe!}5B)Cfts%zx+KRG~>%xK$^Qwgd zxLTF&cv9bZ<-o^?!iTp33(@-}y0vl_)2I~G)HF}92jl3v=#<)O=E7Rmdy2oglf|Hk z9aQ(|WOUn)IJ?Ev)&Fx6bF6)kaLn)DLr)thofgM!&@~!rT^O7bt@e5Or7|3fA$%9Z zdcgKVh^j?@4Sr`l^A=M|1^Pj?Yjm{L@U2TTmvgF?=HwY>`I&p}76Dqk08?8TUT-A< znh@%m4Kr8Z2eAG|xkt@vbVWTWWo?7J*w`48u>8$6i$5I*42V{tPKX%OlI`)B z()!3ib)k>8#b55^e=w>k^6=B1ZbJ{JnSv5WmCH4hd6f6{<7;|@XRLCWiDm99yGhA{}Wd_r0D9El$(X5~ZD@6Yeq%JJ_WnN-%QoT_^& zls{`|J%9EnfAEk~Gyx};)G-ocS*iS3?;B@s#A40HNR)|D9E{SnkxmA^FH@JMsvova zX8&ayT=lAu;Am_z@Z2<%$ z6{BjEfS{w}t2VXwOJOAH%tlL96JNh1#=Wf*>*+C=wXQ*vNIs(oaU1*ElJg~AWBoz3 zYsEM6%XU&tu#eu|7g)PJi(C|&tXR}Xn&hnaxd#gx>+j>0Lv??-YpZnq{-}DCX85D* z_dAPe6Sn6vuwUIAo->ylZ}KI6nQVFL;mT@J6n=}jh-Pjuf%pEv++Pdkdj*Zwp7DBP zrhTtVw6{gZP)&6$66l-|HJ4w8%;eg~^EPs4nrqIew2C|SrN%S*vk{KJr_g;`Y{qKK za@>)WYnnH1$Y7g~H208STe0$q*DizuUr(^Zw@=B+o)Nq~aS$e4yGfT@>daH^DKIsz z)cwow6@MRvpi+T4eltCU8Nd3QQI$>-+ zFgW?Ai;?m(M6`3~3t0MUNoj|7dB1(@$eb{5jiAiUXxRcsX%N7syb8a-?5DbXQY&dt z%QtBuucYrm*1Dmz5?dn{A0>WU5x4YjETQ|M(8cAmSNvkb`2Ku7q`f}>zYyO<$8g@x zUzs*Gk#M%Gr7AVQ9ITO)uwuolOW^S4&Y0P4a>%SIVE2Gjj|O|=mQu{^v#Iu?g_u(5R=+V-wf&8^chyn4kgY80C{bYAldhLKCoo`h9f)w_=Um8!5STwbwF{_U3Tizp{AyHC84h8bs|GATle zy3F*$Rqlvr6MURu^<;ctoJ-C-Z#8oO_$tvow?$;EY1DOi-(^S-2TIzlGbRuwb&;-U z7yZw0dus6-cg1D}!{`%wA1C;jA;~pKJ=T-XVnJJKHRncn?@V?ZaEO-)De!(_{cb?PQZM`vF>be}<$+yB>x!n%iMG>Ka&r0;UkQJwE7Mk z$`Z}Z4DInTXYHvlPzHtdM7q)!O@~4(CDCwL6=&P!6H8`ZIu8IG){ebnFOOsgJi5#J zo*23$q%3OfclD3VJx10L$%S6mR#0O{Z-fm;yH{W~(%~1?#@9nDw|X-FV4WFNNE?u= zNsUpZB9>X39R6ko>B{E@palselu!fi@;2{*99+kw!vd7==a0k1U~mtD?VxZfC+5?F zS*Nm{8|Z%mvGF+lu?k#)zW_q@WO_h`!~-H-wk-C(55!&CS9;0~tCO{qo!|0Ld`A4R z!?{Ezel$sA7!Q>QFASC6a)j3Q9elimCP?JOd_Bc7+iW%D38BRl_}=XI4MruG!$~Y! z)#3G0Ing0FLqmnq)N|04xKy0c__kp$%3zYQ=jbnv1!pZF??**9r5!AA6QQz8HaBac z-)DvlmLg^;&QlPoDNzVPj2csI+Sp_GZ>Y~5tTw?OOsB`~G>Igi6#!z}vZxRn z_mZ3%^Wj5E=1-Gbo9g3YL(lz--KY}z4P9;Rev`lWf1j9yQ{VAp+Sf5=7Hy=={c`2yJadWY-BAQd=Lk{ z_R5BjK&)=KT9B)gLV{1>rb2!28ThgAIW#%qO5B*yF`N0*A1srvHd0v*yWIk#^?)Ts zM5jSs*i*xFz2D7|(i500ZVx1J5^<9kp}e$hGHAC2LutRtBH%ytYf07!h%N|p$O_p@ zM76*Ze2pGv`4>e77PC@W+j}V~VvZ$pJA7_u0J6SUt6#Bso7y)~k6x^LN|Y_7g&3t~ zc23kDyJ8C;xg3Hmb~#>n;^|83;U1180$1Uh7VPe5U0Yj3=xI`|HIDic);1 z%Zx)#tB~cm_w^{1mrymqhd`i6ceMi`P*4dL1i~7qi~&@l{y)Y0VJg~EDlzW{YoEjk QxT*u><#`b=z=h}h2b9~@i2wiq literal 0 HcmV?d00001 diff --git a/common/tier4_localization_rviz_plugin/package.xml b/common/tier4_localization_rviz_plugin/package.xml index f825611b7f3d2..6ef58b8e84dcf 100644 --- a/common/tier4_localization_rviz_plugin/package.xml +++ b/common/tier4_localization_rviz_plugin/package.xml @@ -7,6 +7,7 @@ Takagi, Isamu yabuta Kah Hooi Tan + Takamasa Horibe Apache License 2.0 ament_cmake_auto @@ -21,6 +22,8 @@ rclcpp rviz_common rviz_default_plugins + tf2_ros + vehicle_info_util ament_lint_auto autoware_lint_common diff --git a/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml index 80c5abfdab753..2a58895354cad 100644 --- a/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml +++ b/common/tier4_localization_rviz_plugin/plugins/plugin_description.xml @@ -5,5 +5,9 @@ base_class_type="rviz_common::Display"> Display pose history - + + Display footprint of geometry_msgs::Pose + diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp new file mode 100644 index 0000000000000..5b0266311c0e5 --- /dev/null +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.cpp @@ -0,0 +1,253 @@ +// 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. + +#include "display.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#define EIGEN_MPL2_ONLY +#include +#include + +#include +#include +#include +#include +#include + +namespace rviz_plugins +{ +PoseHistoryFootprint::PoseHistoryFootprint() : last_stamp_(0, 0, RCL_ROS_TIME) +{ + property_buffer_size_ = new rviz_common::properties::IntProperty("Buffer Size", 100, "", this); + + // trajectory footprint + property_trajectory_footprint_view_ = new rviz_common::properties::BoolProperty( + "View Trajectory Footprint", true, "", this, SLOT(updateVisualization()), this); + property_trajectory_footprint_alpha_ = new rviz_common::properties::FloatProperty( + "Alpha", 1.0, "", property_trajectory_footprint_view_, SLOT(updateVisualization()), this); + property_trajectory_footprint_alpha_->setMin(0.0); + property_trajectory_footprint_alpha_->setMax(1.0); + property_trajectory_footprint_color_ = new rviz_common::properties::ColorProperty( + "Color", QColor(230, 230, 50), "", property_trajectory_footprint_view_, + SLOT(updateVisualization()), this); + property_vehicle_length_ = new rviz_common::properties::FloatProperty( + "Vehicle Length", 4.77, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_vehicle_width_ = new rviz_common::properties::FloatProperty( + "Vehicle Width", 1.83, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_rear_overhang_ = new rviz_common::properties::FloatProperty( + "Rear Overhang", 1.03, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_offset_ = new rviz_common::properties::FloatProperty( + "Offset from BaseLink", 0.0, "", property_trajectory_footprint_view_, SLOT(updateVehicleInfo()), + this); + property_interval_ = new rviz_common::properties::FloatProperty( + "Displayed Footprint Interval [m]", 1.0, "", property_trajectory_footprint_view_, + SLOT(updateVehicleInfo()), this); + property_vehicle_length_->setMin(0.0); + property_vehicle_width_->setMin(0.0); + property_rear_overhang_->setMin(0.0); + property_interval_->setMin(0.0); + + updateVehicleInfo(); +} + +PoseHistoryFootprint::~PoseHistoryFootprint() +{ + if (initialized()) { + scene_manager_->destroyManualObject(trajectory_footprint_manual_object_); + } +} + +void PoseHistoryFootprint::updateVehicleInfo() +{ + if (vehicle_info_) { + vehicle_footprint_info_ = std::make_shared( + vehicle_info_->vehicle_length_m, vehicle_info_->vehicle_width_m, + vehicle_info_->rear_overhang_m); + } else { + const float length{property_vehicle_length_->getFloat()}; + const float width{property_vehicle_width_->getFloat()}; + const float rear_overhang{property_rear_overhang_->getFloat()}; + + vehicle_footprint_info_ = std::make_shared(length, width, rear_overhang); + } +} + +void PoseHistoryFootprint::updateVisualization() +{ + if (last_msg_ptr_) { + processMessage(last_msg_ptr_); + } +} + +void PoseHistoryFootprint::onInitialize() +{ + MFDClass::onInitialize(); + + trajectory_footprint_manual_object_ = scene_manager_->createManualObject(); + trajectory_footprint_manual_object_->setDynamic(true); + scene_node_->attachObject(trajectory_footprint_manual_object_); +} + +void PoseHistoryFootprint::onEnable() { subscribe(); } + +void PoseHistoryFootprint::onDisable() { unsubscribe(); } + +void PoseHistoryFootprint::subscribe() { MFDClass::subscribe(); } + +void PoseHistoryFootprint::unsubscribe() +{ + MFDClass::unsubscribe(); + + history_.clear(); +} + +void PoseHistoryFootprint::processMessage( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr message) +{ + if (!rviz_common::validateFloats(message->pose)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Topic", + "Message contained invalid floating point values (nans or infs)"); + return; + } + + updateHistory(message); + updateFootprint(); + + last_msg_ptr_ = message; +} + +void PoseHistoryFootprint::updateHistory( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr message) +{ + if (history_.empty()) { + history_.emplace_back(message); + } else { + const auto dx = message->pose.position.x - history_.back()->pose.position.x; + const auto dy = message->pose.position.y - history_.back()->pose.position.y; + if (std::hypot(dx, dy) > property_interval_->getFloat()) { + history_.emplace_back(message); + } + } + + const auto buffer_size = static_cast(property_buffer_size_->getInt()); + while (buffer_size < history_.size()) { + history_.pop_front(); + } +} + +void PoseHistoryFootprint::updateFootprint() +{ + // This doesn't work in the constructor. + if (!vehicle_info_) { + try { + vehicle_info_ = std::make_shared( + VehicleInfoUtil(*rviz_ros_node_.lock()->get_raw_node()).getVehicleInfo()); + updateVehicleInfo(); + } catch (const std::exception & e) { + RCLCPP_WARN_THROTTLE( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + *rviz_ros_node_.lock()->get_raw_node()->get_clock(), 5000, "Failed to get vehicle_info: %s", + e.what()); + } + } + + if (history_.empty()) return; + + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if (!context_->getFrameManager()->getTransform(history_.back()->header, position, orientation)) { + RCLCPP_DEBUG( + rviz_ros_node_.lock()->get_raw_node()->get_logger(), + "Error transforming from frame '%s' to frame '%s'", history_.back()->header.frame_id.c_str(), + qPrintable(fixed_frame_)); + } + + scene_node_->setPosition(position); + scene_node_->setOrientation(orientation); + + trajectory_footprint_manual_object_->clear(); + + Ogre::MaterialPtr material = Ogre::MaterialManager::getSingleton().getByName( + "BaseWhiteNoLighting", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + material->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA); + material->setDepthWriteEnabled(false); + + if (!history_.empty()) { + trajectory_footprint_manual_object_->estimateVertexCount(history_.size() * 4 * 2); + trajectory_footprint_manual_object_->begin( + "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST); + + const float offset_from_baselink = property_offset_->getFloat(); + + for (size_t point_idx = 0; point_idx < history_.size(); ++point_idx) { + const auto & pose = history_.at(point_idx)->pose; + /* + * Footprint + */ + if (property_trajectory_footprint_view_->getBool()) { + Ogre::ColourValue color; + color = rviz_common::properties::qtToOgre(property_trajectory_footprint_color_->getColor()); + color.a = property_trajectory_footprint_alpha_->getFloat(); + + const auto info = vehicle_footprint_info_; + const float top = info->length - info->rear_overhang - offset_from_baselink; + const float bottom = -info->rear_overhang + offset_from_baselink; + const float left = -info->width / 2.0; + const float right = info->width / 2.0; + + const std::array lon_offset_vec{top, top, bottom, bottom}; + const std::array lat_offset_vec{left, right, right, left}; + + for (int f_idx = 0; f_idx < 4; ++f_idx) { + const auto & o = pose.orientation; + const auto & p = pose.position; + const Eigen::Quaternionf quat(o.w, o.x, o.y, o.z); + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at(f_idx), lat_offset_vec.at(f_idx), 0.0}; + const auto offset_to_edge = quat * offset_vec; + trajectory_footprint_manual_object_->position( + p.x + offset_to_edge.x(), p.y + offset_to_edge.y(), p.z); + trajectory_footprint_manual_object_->colour(color); + } + { + const Eigen::Vector3f offset_vec{ + lon_offset_vec.at((f_idx + 1) % 4), lat_offset_vec.at((f_idx + 1) % 4), 0.0}; + const auto offset_to_edge = quat * offset_vec; + trajectory_footprint_manual_object_->position( + p.x + offset_to_edge.x(), p.y + offset_to_edge.y(), p.z); + trajectory_footprint_manual_object_->colour(color); + } + } + } + } + trajectory_footprint_manual_object_->end(); + } +} + +} // namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::PoseHistoryFootprint, rviz_common::Display) diff --git a/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp new file mode 100644 index 0000000000000..d957054bd479e --- /dev/null +++ b/common/tier4_localization_rviz_plugin/src/pose_history_footprint/display.hpp @@ -0,0 +1,108 @@ +// 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 POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ +#define POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ + +#include +#include + +#include + +#include +#include +#include + +namespace rviz_rendering +{ +class BillboardLine; +} // namespace rviz_rendering +namespace rviz_common::properties +{ +class ColorProperty; +class FloatProperty; +class IntProperty; +class BoolProperty; +} // namespace rviz_common::properties + +namespace rviz_plugins +{ + +using vehicle_info_util::VehicleInfo; +using vehicle_info_util::VehicleInfoUtil; + +class PoseHistoryFootprint +: public rviz_common::MessageFilterDisplay +{ + Q_OBJECT + +public: + PoseHistoryFootprint(); + ~PoseHistoryFootprint() override; + PoseHistoryFootprint(const PoseHistoryFootprint &) = delete; + PoseHistoryFootprint(const PoseHistoryFootprint &&) = delete; + PoseHistoryFootprint & operator=(const PoseHistoryFootprint &) = delete; + PoseHistoryFootprint & operator=(const PoseHistoryFootprint &&) = delete; + +protected: + void onInitialize() override; + void onEnable() override; + void onDisable() override; + void updateFootprint(); + +private Q_SLOTS: + void updateVisualization(); + void updateVehicleInfo(); + +private: + void subscribe() override; + void unsubscribe() override; + void processMessage(const geometry_msgs::msg::PoseStamped::ConstSharedPtr message) override; + void updateHistory(const geometry_msgs::msg::PoseStamped::ConstSharedPtr message); + + std::string target_frame_; + std::deque history_; + rclcpp::Time last_stamp_; + + // pose history + rviz_common::properties::IntProperty * property_buffer_size_; + + // trajectory footprint + Ogre::ManualObject * trajectory_footprint_manual_object_; + rviz_common::properties::BoolProperty * property_trajectory_footprint_view_; + rviz_common::properties::ColorProperty * property_trajectory_footprint_color_; + rviz_common::properties::FloatProperty * property_trajectory_footprint_alpha_; + rviz_common::properties::FloatProperty * property_vehicle_length_; + rviz_common::properties::FloatProperty * property_vehicle_width_; + rviz_common::properties::FloatProperty * property_rear_overhang_; + rviz_common::properties::FloatProperty * property_offset_; + rviz_common::properties::FloatProperty * property_interval_; + + struct VehicleFootprintInfo + { + VehicleFootprintInfo(const float l, const float w, const float r) + : length(l), width(w), rear_overhang(r) + { + } + float length, width, rear_overhang; + }; + std::shared_ptr vehicle_info_; + std::shared_ptr vehicle_footprint_info_; + + geometry_msgs::msg::PoseStamped::ConstSharedPtr last_msg_ptr_; +}; + +} // namespace rviz_plugins + +#endif // POSE_HISTORY_FOOTPRINT__DISPLAY_HPP_ From e8ee04497bae10a67d953b74ff6a4b7c6e6f4d75 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 1 Dec 2022 23:10:35 +0900 Subject: [PATCH 06/13] ci: remove Galactic CI workflows (#2408) * ci: remove Galactic CI workflows Signed-off-by: Kenji Miyake * fix repos name Signed-off-by: Kenji Miyake Signed-off-by: Kenji Miyake --- .github/sync-files.yaml | 6 --- ...ild-and-test-differential-self-hosted.yaml | 6 +-- .../build-and-test-differential.yaml | 8 +--- .../workflows/build-and-test-self-hosted.yaml | 6 +-- .github/workflows/build-and-test.yaml | 6 +-- .github/workflows/check-build-depends.yaml | 6 +-- build_depends.humble.repos | 43 ------------------- build_depends.repos | 2 +- 8 files changed, 7 insertions(+), 76 deletions(-) delete mode 100644 build_depends.humble.repos diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 0af942aee420f..17d0bd6382c44 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -34,7 +34,6 @@ - source: .github/workflows/build-and-test.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} sd -- \ @@ -46,7 +45,6 @@ - source: .github/workflows/build-and-test-differential.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} sd -- \ @@ -60,7 +58,6 @@ - source: .github/workflows/build-and-test-differential-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} sd -- \ @@ -71,7 +68,6 @@ - source: .github/workflows/build-and-test-self-hosted.yaml pre-commands: | sd "container: ros:(\w+)" "container: ghcr.io/autowarefoundation/autoware-universe:\$1-latest" {source} - sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} sd -s 'container: ${{ matrix.container }}' 'container: ${{ matrix.container }}${{ matrix.container-suffix }}' {source} sd -- \ @@ -81,8 +77,6 @@ - -cuda include:" {source} - source: .github/workflows/check-build-depends.yaml - pre-commands: | - sd -f ms '(rosdistro: humble.*?build-depends-repos): build_depends.repos' '$1: build_depends.humble.repos' {source} - source: .github/workflows/clang-tidy-pr-comments.yaml - source: .github/workflows/clang-tidy-pr-comments-manually.yaml - source: .github/workflows/update-codeowners-from-packages.yaml diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index 67fce235ada55..9a527e97a7b46 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -23,17 +23,13 @@ jobs: fail-fast: false matrix: rosdistro: - - galactic - humble container-suffix: - -cuda include: - - rosdistro: galactic - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest - build-depends-repos: build_depends.repos - rosdistro: humble container: ghcr.io/autowarefoundation/autoware-universe:humble-latest - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 43bb8f7bbfea6..60eda66c57df5 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -11,17 +11,13 @@ jobs: fail-fast: false matrix: rosdistro: - - galactic - humble container-suffix: - -cuda include: - - rosdistro: galactic - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest - build-depends-repos: build_depends.repos - rosdistro: humble container: ghcr.io/autowarefoundation/autoware-universe:humble-latest - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 @@ -94,4 +90,4 @@ jobs: target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos diff --git a/.github/workflows/build-and-test-self-hosted.yaml b/.github/workflows/build-and-test-self-hosted.yaml index fd1ba6a50cd1f..9f351a5dc75ec 100644 --- a/.github/workflows/build-and-test-self-hosted.yaml +++ b/.github/workflows/build-and-test-self-hosted.yaml @@ -13,18 +13,14 @@ jobs: fail-fast: false matrix: rosdistro: - - galactic - humble container-suffix: - "" - -cuda include: - - rosdistro: galactic - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest - build-depends-repos: build_depends.repos - rosdistro: humble container: ghcr.io/autowarefoundation/autoware-universe:humble-latest - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index c12b2a7ed34b5..383eb2def936c 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -15,18 +15,14 @@ jobs: fail-fast: false matrix: rosdistro: - - galactic - humble container-suffix: - "" - -cuda include: - - rosdistro: galactic - container: ghcr.io/autowarefoundation/autoware-universe:galactic-latest - build-depends-repos: build_depends.repos - rosdistro: humble container: ghcr.io/autowarefoundation/autoware-universe:humble-latest - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/.github/workflows/check-build-depends.yaml b/.github/workflows/check-build-depends.yaml index 600eaa3c5dcb4..81618a1db0eea 100644 --- a/.github/workflows/check-build-depends.yaml +++ b/.github/workflows/check-build-depends.yaml @@ -13,15 +13,11 @@ jobs: fail-fast: false matrix: rosdistro: - - galactic - humble include: - - rosdistro: galactic - container: ros:galactic - build-depends-repos: build_depends.repos - rosdistro: humble container: ros:humble - build-depends-repos: build_depends.humble.repos + build-depends-repos: build_depends.repos steps: - name: Check out repository uses: actions/checkout@v3 diff --git a/build_depends.humble.repos b/build_depends.humble.repos deleted file mode 100644 index 5e8ce5953172a..0000000000000 --- a/build_depends.humble.repos +++ /dev/null @@ -1,43 +0,0 @@ -repositories: - # core - core/autoware_common: - type: git - url: https://github.com/autowarefoundation/autoware_common.git - version: main - core/autoware.core: - type: git - url: https://github.com/autowarefoundation/autoware.core.git - version: main - core/autoware_msgs: - type: git - url: https://github.com/autowarefoundation/autoware_msgs.git - version: main - core/autoware_adapi_msgs: - type: git - url: https://github.com/autowarefoundation/autoware_adapi_msgs.git - version: main - core/external/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main - # universe - universe/external/tier4_autoware_msgs: - type: git - url: https://github.com/tier4/tier4_autoware_msgs.git - version: tier4/universe - universe/external/grid_map: - type: git - url: https://github.com/ANYbotics/grid_map.git - version: 74333f037cfce321248dfa7b954a815d4f67d79d - universe/external/mussp: - type: git - url: https://github.com/tier4/muSSP.git - version: tier4/main - universe/external/ndt_omp: - type: git - url: https://github.com/tier4/ndt_omp.git - version: tier4/main - universe/external/morai_msgs: - type: git - url: https://github.com/MORAI-Autonomous/MORAI-ROS2_morai_msgs.git - version: main diff --git a/build_depends.repos b/build_depends.repos index 2b47af3ef8e81..5e8ce5953172a 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -28,7 +28,7 @@ repositories: universe/external/grid_map: type: git url: https://github.com/ANYbotics/grid_map.git - version: ba2f9cb6e62f7ee9c5bac7401391a211e442e459 + version: 74333f037cfce321248dfa7b954a815d4f67d79d universe/external/mussp: type: git url: https://github.com/tier4/muSSP.git From 3aca1eaa39627fa3be173dc3196b4aa3a6949bb5 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 1 Dec 2022 23:14:12 +0900 Subject: [PATCH 07/13] revert: revert "ci(build-and-test-differential): disable no-cuda test" (#2410) Revert "ci(build-and-test-differential): disable no-cuda test (#1669)" This reverts commit 2d62bdf127b8215c73be6416c57861d4a812ef0b. --- .github/sync-files.yaml | 2 ++ .github/workflows/build-and-test-differential-self-hosted.yaml | 1 + .github/workflows/build-and-test-differential.yaml | 1 + 3 files changed, 4 insertions(+) diff --git a/.github/sync-files.yaml b/.github/sync-files.yaml index 17d0bd6382c44..656d632f00f8c 100644 --- a/.github/sync-files.yaml +++ b/.github/sync-files.yaml @@ -50,6 +50,7 @@ sd -- \ " include:" \ " container-suffix: + - \"\" - -cuda include:" {source} @@ -63,6 +64,7 @@ sd -- \ " include:" \ " container-suffix: + - \"\" - -cuda include:" {source} - source: .github/workflows/build-and-test-self-hosted.yaml diff --git a/.github/workflows/build-and-test-differential-self-hosted.yaml b/.github/workflows/build-and-test-differential-self-hosted.yaml index 9a527e97a7b46..86d3011cf3208 100644 --- a/.github/workflows/build-and-test-differential-self-hosted.yaml +++ b/.github/workflows/build-and-test-differential-self-hosted.yaml @@ -25,6 +25,7 @@ jobs: rosdistro: - humble container-suffix: + - "" - -cuda include: - rosdistro: humble diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index 60eda66c57df5..c114dac59e325 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -13,6 +13,7 @@ jobs: rosdistro: - humble container-suffix: + - "" - -cuda include: - rosdistro: humble From baa5a95b2e64d416ec7e3e607f7d175a139d0fa5 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 1 Dec 2022 23:18:49 +0900 Subject: [PATCH 08/13] ci(build_depends.repos): remove grid_map (#2429) * ci: remove Galactic CI workflows Signed-off-by: Kenji Miyake * ci(build_depends.repos): remove grid_map Signed-off-by: Kenji Miyake Signed-off-by: Kenji Miyake --- build_depends.repos | 4 ---- 1 file changed, 4 deletions(-) diff --git a/build_depends.repos b/build_depends.repos index 5e8ce5953172a..1193d710b91f4 100644 --- a/build_depends.repos +++ b/build_depends.repos @@ -25,10 +25,6 @@ repositories: type: git url: https://github.com/tier4/tier4_autoware_msgs.git version: tier4/universe - universe/external/grid_map: - type: git - url: https://github.com/ANYbotics/grid_map.git - version: 74333f037cfce321248dfa7b954a815d4f67d79d universe/external/mussp: type: git url: https://github.com/tier4/muSSP.git From 462338840ec27b2d1c363427ac95eb49b9f91a0d Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Fri, 2 Dec 2022 00:06:58 +0900 Subject: [PATCH 09/13] feat(radar_scan_to_pointcloud2): add radar_scan_to_pointcloud2 package (#2336) * feat(radar_scan_to_pointcloud2): add radar_scan_to_pointcloud2 package Signed-off-by: scepter914 * apply pre-commit Signed-off-by: scepter914 * fix parameters Signed-off-by: scepter914 * apply pre-commit Signed-off-by: scepter914 * make temporary variable Signed-off-by: scepter914 * use autoware_cmame in CMakeLists.txt Signed-off-by: scepter914 * refactor Signed-off-by: scepter914 * add dependancy Signed-off-by: scepter914 * fix build error at galactic enviroment Signed-off-by: scepter914 * apply cpplint Signed-off-by: scepter914 * add code maintainer Signed-off-by: scepter914 * apply cpplint Signed-off-by: scepter914 Signed-off-by: scepter914 --- .../radar_scan_to_pointcloud2/CMakeLists.txt | 30 ++++ sensing/radar_scan_to_pointcloud2/README.md | 33 ++++ .../radar_scan_to_pointcloud2_node.hpp | 70 ++++++++ .../radar_scan_to_pointcloud2.launch.xml | 15 ++ sensing/radar_scan_to_pointcloud2/package.xml | 28 ++++ .../radar_scan_to_pointcloud2_node.cpp | 154 ++++++++++++++++++ 6 files changed, 330 insertions(+) create mode 100644 sensing/radar_scan_to_pointcloud2/CMakeLists.txt create mode 100644 sensing/radar_scan_to_pointcloud2/README.md create mode 100644 sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp create mode 100644 sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml create mode 100644 sensing/radar_scan_to_pointcloud2/package.xml create mode 100644 sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp diff --git a/sensing/radar_scan_to_pointcloud2/CMakeLists.txt b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt new file mode 100644 index 0000000000000..1fb63df6594bf --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.5) +project(radar_scan_to_pointcloud2) + +# Dependencies +find_package(autoware_cmake REQUIRED) +autoware_package() + +# Targets +ament_auto_add_library(radar_scan_to_pointcloud2_node_component SHARED + src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp +) + +rclcpp_components_register_node(radar_scan_to_pointcloud2_node_component + PLUGIN "radar_scan_to_pointcloud2::RadarScanToPointcloud2Node" + EXECUTABLE radar_scan_to_pointcloud2_node +) + +# Tests +if(BUILD_TESTING) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +# Package +ament_auto_package( + INSTALL_TO_SHARE + launch +) diff --git a/sensing/radar_scan_to_pointcloud2/README.md b/sensing/radar_scan_to_pointcloud2/README.md new file mode 100644 index 0000000000000..100fd509ef35b --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/README.md @@ -0,0 +1,33 @@ +# radar_scan_to_pointcloud2 + +## radar_scan_to_pointcloud2_node + +- Convert from `radar_msgs::msg::RadarScan` to `sensor_msgs::msg::PointCloud2` +- Calculation cost O(n) + - n: The number of radar return + +### Input topics + +| Name | Type | Description | +| ----------- | -------------------------- | ----------- | +| input/radar | radar_msgs::msg::RadarScan | RadarScan | + +### Output topics + +| Name | Type | Description | +| --------------------------- | ----------------------------- | ----------------------------------------------------------------- | +| output/amplitude_pointcloud | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud whose intensity is amplitude. | +| output/doppler_pointcloud | sensor_msgs::msg::PointCloud2 | PointCloud2 radar pointcloud whose intensity is doppler velocity. | + +### Parameters + +| Name | Type | Description | +| ---------------------------- | ---- | ----------------------------------------------------------------------------------------- | +| publish_amplitude_pointcloud | bool | Whether publish radar pointcloud whose intensity is amplitude. Default is `true`. | +| publish_doppler_pointcloud | bool | Whether publish radar pointcloud whose intensity is doppler velocity. Default is `false`. | + +### How to launch + +```sh +ros2 launch radar_scan_to_pointcloud2 radar_scan_to_pointcloud2.launch.xml +``` diff --git a/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp new file mode 100644 index 0000000000000..bb331609f606d --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/include/radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp @@ -0,0 +1,70 @@ +// 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 RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ +#define RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +#include +#include +#include + +namespace radar_scan_to_pointcloud2 +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; +using sensor_msgs::msg::PointCloud2; + +class RadarScanToPointcloud2Node : public rclcpp::Node +{ +public: + explicit RadarScanToPointcloud2Node(const rclcpp::NodeOptions & node_options); + + struct NodeParam + { + bool publish_amplitude_pointcloud{}; + bool publish_doppler_pointcloud{}; + }; + +private: + // Subscriber + rclcpp::Subscription::SharedPtr sub_radar_{}; + + // Callback + void onData(const RadarScan::ConstSharedPtr radar_msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_amplitude_pointcloud_{}; + rclcpp::Publisher::SharedPtr pub_doppler_pointcloud_{}; + + // Parameter Server + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onSetParam( + const std::vector & params); + + // Data buffer + sensor_msgs::msg::PointCloud2 amplitude_pointcloud; + sensor_msgs::msg::PointCloud2 doppler_pointcloud; + + // Parameter + NodeParam node_param_{}; +}; + +} // namespace radar_scan_to_pointcloud2 + +#endif // RADAR_SCAN_TO_POINTCLOUD2__RADAR_SCAN_TO_POINTCLOUD2_NODE_HPP_ diff --git a/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml new file mode 100644 index 0000000000000..f94b0bfd91ea8 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/launch/radar_scan_to_pointcloud2.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/sensing/radar_scan_to_pointcloud2/package.xml b/sensing/radar_scan_to_pointcloud2/package.xml new file mode 100644 index 0000000000000..dc6ba43812ce6 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/package.xml @@ -0,0 +1,28 @@ + + + radar_scan_to_pointcloud2 + 0.1.0 + radar_scan_to_pointcloud2 + Satoshi Tanaka + Shunsuke Miura + Satoshi Tanaka + Apache License 2.0 + + ament_cmake_auto + + pcl_conversions + pcl_ros + radar_msgs + rclcpp + rclcpp_components + sensor_msgs + + autoware_cmake + ament_clang_format + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp new file mode 100644 index 0000000000000..de4ac74db6429 --- /dev/null +++ b/sensing/radar_scan_to_pointcloud2/src/radar_scan_to_pointcloud2_node/radar_scan_to_pointcloud2_node.cpp @@ -0,0 +1,154 @@ +// 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. + +#include "radar_scan_to_pointcloud2/radar_scan_to_pointcloud2_node.hpp" + +#include +#include +#include +#include + +#include +#include + +namespace +{ +template +bool update_param( + const std::vector & params, const std::string & name, T & value) +{ + const auto itr = std::find_if( + params.cbegin(), params.cend(), + [&name](const rclcpp::Parameter & p) { return p.get_name() == name; }); + // Not found + if (itr == params.cend()) { + return false; + } + value = itr->template get_value(); + return true; +} + +pcl::PointXYZI getPointXYZI(const radar_msgs::msg::RadarReturn & radar, float intensity) +{ + pcl::PointXYZI point; + const float r_xy = radar.range * std::cos(radar.elevation); + point.x = r_xy * std::cos(radar.azimuth); + point.y = r_xy * std::sin(radar.azimuth); + point.z = radar.range * std::sin(radar.elevation); + point.intensity = intensity; + return point; +} + +pcl::PointCloud toAmplitudePCL(const radar_msgs::msg::RadarScan & radar_scan) +{ + pcl::PointCloud pcl; + for (const auto & radar : radar_scan.returns) { + pcl.push_back(getPointXYZI(radar, radar.amplitude)); + } + return pcl; +} + +sensor_msgs::msg::PointCloud2 toAmplitudePointcloud2(const radar_msgs::msg::RadarScan & radar_scan) +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + auto pcl_pointcloud = toAmplitudePCL(radar_scan); + pcl::toROSMsg(pcl_pointcloud, pointcloud_msg); + pointcloud_msg.header = radar_scan.header; + return pointcloud_msg; +} + +pcl::PointCloud toDopplerPCL(const radar_msgs::msg::RadarScan & radar_scan) +{ + pcl::PointCloud pcl; + for (const auto & radar : radar_scan.returns) { + pcl.push_back(getPointXYZI(radar, radar.doppler_velocity)); + } + return pcl; +} + +sensor_msgs::msg::PointCloud2 toDopplerPointcloud2(const radar_msgs::msg::RadarScan & radar_scan) +{ + sensor_msgs::msg::PointCloud2 pointcloud_msg; + auto pcl_pointcloud = toDopplerPCL(radar_scan); + pcl::toROSMsg(pcl_pointcloud, pointcloud_msg); + pointcloud_msg.header = radar_scan.header; + return pointcloud_msg; +} +} // namespace + +namespace radar_scan_to_pointcloud2 +{ +using radar_msgs::msg::RadarReturn; +using radar_msgs::msg::RadarScan; +using sensor_msgs::msg::PointCloud2; + +RadarScanToPointcloud2Node::RadarScanToPointcloud2Node(const rclcpp::NodeOptions & node_options) +: Node("radar_scan_to_pointcloud2", node_options) +{ + using std::placeholders::_1; + + // Parameter Server + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&RadarScanToPointcloud2Node::onSetParam, this, _1)); + + // Node Parameter + node_param_.publish_amplitude_pointcloud = + declare_parameter("publish_amplitude_pointcloud", true); + node_param_.publish_doppler_pointcloud = + declare_parameter("publish_doppler_pointcloud", false); + + // Subscriber + sub_radar_ = create_subscription( + "~/input/radar", rclcpp::QoS{1}, std::bind(&RadarScanToPointcloud2Node::onData, this, _1)); + + // Publisher + pub_amplitude_pointcloud_ = create_publisher("~/output/amplitude_pointcloud", 1); + pub_doppler_pointcloud_ = create_publisher("~/output/doppler_pointcloud", 1); +} + +rcl_interfaces::msg::SetParametersResult RadarScanToPointcloud2Node::onSetParam( + const std::vector & params) +{ + rcl_interfaces::msg::SetParametersResult result; + + try { + auto & p = node_param_; + update_param(params, "publish_amplitude_pointcloud", p.publish_amplitude_pointcloud); + update_param(params, "publish_doppler_pointcloud", p.publish_doppler_pointcloud); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + return result; + } + result.successful = true; + result.reason = "success"; + return result; +} + +void RadarScanToPointcloud2Node::onData(const RadarScan::ConstSharedPtr radar_msg) +{ + if (node_param_.publish_amplitude_pointcloud) { + amplitude_pointcloud = toAmplitudePointcloud2(*radar_msg); + pub_amplitude_pointcloud_->publish(amplitude_pointcloud); + } + if (node_param_.publish_doppler_pointcloud) { + doppler_pointcloud = toDopplerPointcloud2(*radar_msg); + pub_doppler_pointcloud_->publish(doppler_pointcloud); + } +} + +} // namespace radar_scan_to_pointcloud2 + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(radar_scan_to_pointcloud2::RadarScanToPointcloud2Node) From c2f47b2d2b1aa07f64fc305b9ed14b244906a153 Mon Sep 17 00:00:00 2001 From: YueChuCheng <39110560+YueChuCheng@users.noreply.github.com> Date: Fri, 2 Dec 2022 00:27:05 +0800 Subject: [PATCH 10/13] feat(tier4_perception_launch): add centerpoint_vs_centerpoint-tiny launch file (#2304) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Add centerpoint_vs_centerpoint-tiny launch file This launch file is for showing centerpoint and centerpoint_tinymodels’ results simultaneously, making it easier to visualize and compare the performance. Signed-off-by: YueChuCheng * ci(pre-commit): autofix * Relocate folder from tier4_perception_launch to lidar_centerpoint. Signed-off-by: YueChuCheng * ci(pre-commit): autofix * Correct XML format Signed-off-by: YueChuCheng * ci(pre-commit): autofix * Remove redundant code. Add sample rosbag link provided by autoware. Signed-off-by: YueChuCheng * Correct spelling mistake. Signed-off-by: YueChuCheng * Rename README file Signed-off-by: YueChuCheng Signed-off-by: YueChuCheng Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Shunsuke Miura <37187849+miursh@users.noreply.github.com> --- .../centerpoint_vs_centerpoint-tiny/README.md | 140 ++ ...centerpoint_vs_centerpoint-tiny.launch.xml | 43 + .../rviz/centerpoint.rviz | 1836 ++++++++++++++++ .../rviz/centerpoint_tiny.rviz | 1837 +++++++++++++++++ 4 files changed, 3856 insertions(+) create mode 100644 perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md create mode 100644 perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml create mode 100644 perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz create mode 100644 perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md new file mode 100644 index 0000000000000..c78ac8d235b5d --- /dev/null +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/README.md @@ -0,0 +1,140 @@ +# Run lidar_centerpoint and lidar_centerpoint-tiny simultaneously + +This tutorial is for showing `centerpoint` and `centerpoint_tiny`models’ results simultaneously, making it easier to visualize and compare the performance. + +## Setup Development Environment + +Follow the steps in the Source Installation ([link](https://autowarefoundation.github.io/autoware-documentation/main/installation/autoware/source-installation/)) in Autoware doc. + +If you fail to build autoware environment according to lack of memory, then it is recommended to build autoware sequentially. + +Source the ROS 2 Galactic setup script. + +```bash +source /opt/ros/galactic/setup.bash +``` + +Build the entire autoware repository. + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers=1 +``` + +Or you can use a constrained number of CPU to build only one package. + +```bash +export MAKEFLAGS="-j 4" && MAKE_JOBS=4 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release --parallel-workers 1 --packages-select PACKAGE_NAME +``` + +Source the package. + +```bash +source install/setup.bash +``` + +## Data Preparation + +### Using rosbag dataset + +```bash +ros2 bag play /YOUR/ROSBAG/PATH/ --clock 100 +``` + +Don't forget to add `clock` inorder to sync between two rviz display. + +You can also use the sample rosbag provided by autoware [here](https://autowarefoundation.github.io/autoware-documentation/main/tutorials/ad-hoc-simulation/rosbag-replay-simulation/). + +If you want to merge several rosbags into one, you can refer to [this tool](https://github.com/jerry73204/rosbag2-merge). + +### Using realtime LiDAR dataset + +Set up your Ethernet connection according to 1.1 - 1.3 in [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16). + +Download Velodyne ROS driver + +```bash +git clone -b ros2 https://github.com/ros-drivers/velodyne.git +``` + +Source the ROS 2 Galactic setup script. + +```bash +source /opt/ros/galactic/setup.bash +``` + +Compile Velodyne driver + +```bash +cd velodyne +rosdep install -y --from-paths . --ignore-src --rosdistro $ROS_DISTRO +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +Edit the configuration file. Specify the LiDAR device IP address in `./velodyne_driver/config/VLP32C-velodyne_driver_node-params.yaml` + +```yaml +velodyne_driver_node: + ros__parameters: + device_ip: 192.168.1.201 //change to your LiDAR device IP address + gps_time: false + time_offset: 0.0 + enabled: true + read_once: false + read_fast: false + repeat_delay: 0.0 + frame_id: velodyne + model: 32C + rpm: 600.0 + port: 2368 +``` + +Launch the velodyne driver. + +```bash +# Terminal 1 +ros2 launch velodyne_driver velodyne_driver_node-VLP32C-launch.py +``` + +Launch the velodyne_pointcloud. + +```bash +# Terminal 2 +ros2 launch velodyne_pointcloud velodyne_convert_node-VLP32C-launch.py +``` + +Point Cloud data will be available on topic `/velodyne_points`. You can check with `ros2 topic echo /velodyne_points`. + +Check [this website](http://wiki.ros.org/velodyne/Tutorials/Getting%20Started%20with%20the%20Velodyne%20VLP16) if there is any unexpected issue. + +## Launch file setting + +Several fields to check in `centerpoint_vs_centerpoint-tiny.launch.xml` before running lidar centerpoint. + +- `input/pointcloud` : set to the topic with input data you want to subscribe. +- `model_path` : set to the path of the model. +- `model_param_path` : set to the path of model's config file. + +## Run CenterPoint and CenterPoint-tiny simultaneously + +Run + +```bash +ros2 launch lidar_centerpoint centerpoint_vs_centerpoint-tiny.launch.xml +``` + +Then you will see two rviz window show immediately. On the left is the result for lidar centerpoint tiny, and on the right is the result for lidar centerpoint. + +![two rviz2 display centerpoint and centerpoint_tiny](https://i.imgur.com/YAYehrf.jpg) + +## Troubleshooting + +### Bounding Box blink on rviz + +To avoid Bounding Boxs blinking on rviz, you can extend bbox marker lifetime. + +Set `marker_ptr->lifetime` and `marker.lifetime` to a longer lifetime. + +- `marker_ptr->lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp` +- `marker.lifetime` are in `PATH/autoware/src/universe/autoware.universe/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp` + +Make sure to rebuild packages after any change. diff --git a/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml new file mode 100644 index 0000000000000..13fd386238eda --- /dev/null +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/centerpoint_vs_centerpoint-tiny.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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 new file mode 100644 index 0000000000000..2685bf96eb881 --- /dev/null +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint.rviz @@ -0,0 +1,1836 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Sensing1/LiDAR1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Perception1/ObjectRecognition1/Detection1 + - /ConcatenatePointCloud1/Autocompute Value Bounds1 + Splitter Ratio: 0.3668779730796814 + Tree Height: 434 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 64 + Length: 128 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 256 + Length: 128 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 298 + Length: 48 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 140 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 128 + Left: 98 + Name: TurnSignal + Top: 175 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 256 + Enabled: true + Name: Vehicle + Enabled: false + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: false + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4.560525852145117e-41 + Min Color: 0; 0; 0 + Min Intensity: 7.707141553786494e-44 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 2 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: false + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: false + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/temporary_merged_objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: true + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: false + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/debug/rois + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: false + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate + Value: true + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: true + Value: true + Width: 2 + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Avoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: true + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: true + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: ModifiedGoal + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: false + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: true + View Path: + Alpha: 1 + Color: 255; 255; 255 + Constant Color: true + Value: true + Width: 0.05000000074505806 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/pure_pursuit/debug/markers + Value: false + Enabled: false + Name: Control + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.07999999821186066 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Name: centerpoint + Namespaces: + label: true + shape: true + twist: true + velocity: true + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /centerpoint/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4.560525852145117e-41 + Min Color: 0; 0; 0 + Min Intensity: 7.707141553786494e-44 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 2 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 50.32217788696289 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6553983092308044 + Target Frame: base_link + Value: XYOrbit (rviz_default_plugins) + Yaw: 3.20540452003479 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: true + Displays: + collapsed: true + Height: 1028 + Hide Left Dock: true + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: true + Views: + collapsed: true + Width: 928 + X: 992 + Y: 24 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 new file mode 100644 index 0000000000000..a744e1ffdeb46 --- /dev/null +++ b/perception/lidar_centerpoint/launch/centerpoint_vs_centerpoint-tiny/rviz/centerpoint_tiny.rviz @@ -0,0 +1,1837 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Sensing1/LiDAR1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1 + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Perception1/ObjectRecognition1/Detection1 + - /centerpoint_tiny1 + - /ConcatenatePointCloud1/Autocompute Value Bounds1 + Splitter Ratio: 0.3668779730796814 + Tree Height: 451 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 64 + Length: 128 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 256 + Length: 128 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 298 + Length: 48 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 140 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 128 + Left: 98 + Name: TurnSignal + Top: 175 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 256 + Enabled: true + Name: Vehicle + Enabled: false + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: false + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4.560525852145117e-41 + Min Color: 0; 0; 0 + Min Intensity: 7.707141553786494e-44 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 2 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: false + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.019999999552965164 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: false + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/temporary_merged_objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: true + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: false + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/debug/rois + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: false + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: RouteArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/route_marker + Value: true + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.5 + Name: GoalPose + Shaft Length: 3 + Shaft Radius: 0.20000000298023224 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/echo_back_goal_pose + Value: true + Enabled: true + Name: MissionPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: ScenarioTrajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/trajectory + Value: true + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Path: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Velocity: + Alpha: 0.4000000059604645 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: PathChangeCandidate + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/output/path_candidate + Value: true + View Path: + Alpha: 0.30000001192092896 + Color: 115; 210; 22 + Constant Color: true + Value: true + Width: 2 + View Velocity: + Alpha: 0.30000001192092896 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (BlindSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/blind_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Crosswalk) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/crosswalk + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (DetectionArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/detection_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (Intersection) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/intersection + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (MergeFromPrivateArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/merge_from_private + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (NoStoppingArea) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/no_stopping_area + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (OcclusionSpot) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/occlusion_spot + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (StopLine) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/stop_line + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (TrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (VirtualTrafficLight) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/virtual_traffic_light + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (RunOut) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_wall/run_out + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Arrow + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/path + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Crosswalk + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Intersection + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Blind Spot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/blind_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: TrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: VirtualTrafficLight + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/virtual_traffic_light + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: StopLine + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/stop_line + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: DetectionArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/detection_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: OcclusionSpot + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/occlusion_spot + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: NoStoppingArea + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/no_stopping_area + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: RunOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/run_out + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Avoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneChange + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: LaneFollowing + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanefollowing + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullover + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: PullOut + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/pullout + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: SideShift + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/sideshift + Value: false + Enabled: false + Name: DebugMarker + Enabled: true + Name: BehaviorPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/trajectory + Value: false + View Path: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 2 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 0.9990000128746033 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: true + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleStop) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (SurroundObstacle) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/virtual_wall + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: VirtualWall (ObstacleAvoidance) + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/wall_marker + Value: true + Enabled: true + Name: VirtualWall + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: SurroundObstacleCheck + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: Footprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 239; 41; 41 + Enabled: false + Name: FootprintOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_offset + Value: false + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 10; 21; 255 + Enabled: false + Name: FootprintRecoverOffset + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/footprint_recover_offset + Value: false + Enabled: true + Name: SurroundObstacleChecker + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleStop + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/debug/marker + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: ObstacleAvoidance + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: true + Enabled: false + Name: DebugMarker + Enabled: true + Name: MotionPlanning + Enabled: true + Name: LaneDriving + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: Costmap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/costmap_generator/occupancy_grid_updates + Use Timestamp: false + Value: false + - Alpha: 0.9990000128746033 + Arrow Length: 0.30000001192092896 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PartialPoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (3D) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/partial_pose_array + Value: true + - Alpha: 0.9990000128746033 + Arrow Length: 0.5 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: true + Head Length: 0.10000000149011612 + Head Radius: 0.20000000298023224 + Name: PoseArray + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.05000000074505806 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/parking/freespace_planner/debug/pose_array + Value: true + Enabled: true + Name: Parking + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.30000001192092896 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: ModifiedGoal + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/modified_goal + Value: true + Enabled: true + Name: ScenarioPlanning + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: PlanningErrorMarker + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /planning/planning_diagnostics/planning_error_monitor/debug/marker + Value: true + Enabled: false + Name: Diagnostic + Enabled: false + Name: Planning + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Predicted Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/lateral/predicted_trajectory + Value: true + View Path: + Alpha: 1 + Color: 255; 255; 255 + Constant Color: true + Value: true + Width: 0.05000000074505806 + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/MPC + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/mpc_follower/debug/markers + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Debug/PurePursuit + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /control/trajectory_follower/pure_pursuit/debug/markers + Value: false + Enabled: false + Name: Control + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display 3d polygon: true + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.07999999821186066 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Name: centerpoint_tiny + Namespaces: + label: true + shape: true + twist: true + velocity: true + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /centerpoint_tiny/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4.560525852145117e-41 + Min Color: 0; 0; 0 + Min Intensity: 7.707141553786494e-44 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 2 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/XYOrbit + Distance: 50.32217788696289 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6553983092308044 + Target Frame: base_link + Value: XYOrbit (rviz_default_plugins) + Yaw: 3.20540452003479 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: true + Displays: + collapsed: true + Height: 1028 + Hide Left Dock: true + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: true + Views: + collapsed: true + Width: 928 + X: 65 + Y: 24 From 40aa7e383855dd305e693a974f9bd06f290814bd Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Fri, 2 Dec 2022 02:32:10 +0300 Subject: [PATCH 11/13] feat(behavior_path_planner): update path when object is gone (#2314) * feat(behavior_path_planner): update state with obstacles. Signed-off-by: ismet atabay feat(behavior_path_planner): update path when obstacle is gone Signed-off-by: ismet atabay Signed-off-by: ismetatabay * ci(pre-commit): autofix Signed-off-by: ismetatabay * update check mechanism Signed-off-by: ismetatabay update check mechanism Signed-off-by: ismetatabay update check mechanism Signed-off-by: ismetatabay * readme.md is updated Signed-off-by: ismetatabay * ci(pre-commit): autofix Signed-off-by: ismetatabay * avoidance maneuver checker is added. Signed-off-by: ismet atabay ci(pre-commit): autofix avoidance maneuver checker is added. Signed-off-by: ismet atabay Signed-off-by: ismetatabay * fix check algorithm Signed-off-by: ismetatabay fix check algorithm Signed-off-by: ismetatabay * documentation is updated. Signed-off-by: ismetatabay * ci(pre-commit): autofix * fix typos Signed-off-by: ismetatabay Signed-off-by: ismet atabay Signed-off-by: ismetatabay Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../avoidance/avoidance.param.yaml | 1 + .../behavior_path_planner_avoidance-design.md | 11 +++++++++++ .../config/avoidance/avoidance.param.yaml | 1 + .../avoidance/avoidance_module.hpp | 2 ++ .../avoidance/avoidance_module_data.hpp | 3 +++ .../src/behavior_path_planner_node.cpp | 1 + .../avoidance/avoidance_module.cpp | 19 +++++++++++++++++++ 7 files changed, 38 insertions(+) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index c592631167a43..93a1e498a757a 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -9,6 +9,7 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false threshold_distance_object_is_on_center: 1.0 # [m] threshold_speed_object_is_stopped: 1.0 # [m/s] diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 03b488423c8b9..cefc482be4896 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -380,6 +380,16 @@ The shift points are modified by a filtering process in order to get the expecte - Similar gradient removal: Connect two shift points with a straight line, and remove the shift points in between if their shift amount is in the vicinity of the straight line. - Remove momentary returns: For shift points that reduce the avoidance width (for going back to the center line), if there is enough long distance in the longitudinal direction, remove them. +##### Avoidance Cancelling + +If `enable_update_path_when_object_is_gone` parameter is true, Avoidance Module takes different actions according to the situations as follows: + +- If vehicle stops: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is stopping, the avoidance path will cancelled. +- If vehicle is in motion, but avoidance maneuver doesn't started: If there is any object in the path of the vehicle, the avoidance path is generated. If this object goes away while the vehicle is not started avoidance maneuver, the avoidance path will cancelled. +- If vehicle is in motion, avoidance maneuver started: If there is any object in the path of the vehicle, the avoidance path is generated,but if this object goes away while the vehicle is started avoidance maneuver, the avoidance path will not cancelled. + +If `enable_update_path_when_object_is_gone` parameter is false, Avoidance Module doesn't revert generated avoidance path even if path objects are gone. + #### How to keep the consistency of the planning TODO @@ -411,6 +421,7 @@ The avoidance specific parameter configuration file can be located at `src/autow | avoidance_execution_lateral_threshold | [m] | double | The lateral distance deviation threshold between the current path and suggested avoidance point to execute avoidance. (\*2) | 0.499 | | enable_avoidance_over_same_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | | enable_avoidance_over_opposite_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true | +| enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | (\*2) 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. diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 7967505b8cc2f..5cbd4301ea197 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -9,6 +9,7 @@ enable_avoidance_over_same_direction: true enable_avoidance_over_opposite_direction: true + enable_update_path_when_object_is_gone: false threshold_distance_object_is_on_center: 1.0 # [m] threshold_speed_object_is_stopped: 1.0 # [m/s] 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 56e482f151752..ddd20f09aa5d7 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 @@ -213,6 +213,8 @@ class AvoidanceModule : public SceneModuleInterface void updateRegisteredRawShiftLines(); // -- for state management -- + bool is_avoidance_maneuver_starts; + bool isAvoidanceManeuverRunning(); bool isAvoidancePlanRunning() const; // -- for pre-processing -- diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 87253151e1493..50fba4078d194 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -64,6 +64,9 @@ struct AvoidanceParameters // to use this, enable_avoidance_over_same_direction need to be set to true. bool enable_avoidance_over_opposite_direction{true}; + // enable update path when if detected objects on planner data is gone. + bool enable_update_path_when_object_is_gone{false}; + // 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; 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 467fd407992e7..8a72a8da9061c 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -278,6 +278,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0); p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true); p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true); + p.enable_update_path_when_object_is_gone = dp("enable_update_path_when_object_is_gone", false); p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0); p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0); 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 aba4ef8e29bd9..79c692d1de4ad 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 @@ -103,8 +103,14 @@ BT::NodeStatus AvoidanceModule::updateState() DebugData debug; const auto avoid_data = calcAvoidancePlanningData(debug); const bool has_avoidance_target = !avoid_data.target_objects.empty(); + if (!is_plan_running && !has_avoidance_target) { current_state_ = BT::NodeStatus::SUCCESS; + } else if ( + !has_avoidance_target && parameters_->enable_update_path_when_object_is_gone && + !isAvoidanceManeuverRunning()) { + // if dynamic objects are removed on path, change current state to reset path + current_state_ = BT::NodeStatus::SUCCESS; } else { current_state_ = BT::NodeStatus::RUNNING; } @@ -121,6 +127,18 @@ bool AvoidanceModule::isAvoidancePlanRunning() const const bool has_shift_line = (path_shifter_.getShiftLinesSize() > 0); return has_base_offset || has_shift_line; } +bool AvoidanceModule::isAvoidanceManeuverRunning() +{ + const auto path_idx = avoidance_data_.ego_closest_path_index; + + for (const auto & al : registered_raw_shift_lines_) { + if (path_idx > al.start_idx || is_avoidance_maneuver_starts) { + is_avoidance_maneuver_starts = true; + return true; + } + } + return false; +} AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & debug) const { @@ -2667,6 +2685,7 @@ void AvoidanceModule::initVariables() registered_raw_shift_lines_ = {}; current_raw_shift_lines_ = {}; original_unique_id = 0; + is_avoidance_maneuver_starts = false; } bool AvoidanceModule::isTargetObjectType(const PredictedObject & object) const From cd729c5904f29b5086a6983667b03a7e12be5a37 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 2 Dec 2022 09:39:01 +0900 Subject: [PATCH 12/13] feat(avoidance): improve avoidance target filter (#2329) * feat(route_handler): add getMostLeftLanelet() Signed-off-by: satoshi-ota * feat(avoidance): calc shiftable ratio in avoidance target filtering process Signed-off-by: satoshi-ota * feat(avoidance): output object's debug info for rviz Signed-off-by: satoshi-ota * fix(avoidance): use avoidance debug factor Signed-off-by: satoshi-ota * feat(tier4_planning_launch): add new params for avoidance Signed-off-by: satoshi-ota * fix(avoidance): reorder params for readability Signed-off-by: satoshi-ota * fix(tier4_planning_launch): reorder params for readability Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance.param.yaml | 10 +- .../config/avoidance/avoidance.param.yaml | 12 +- .../avoidance/avoidance_module_data.hpp | 13 ++ .../scene_module/avoidance/debug.hpp | 1 + .../src/behavior_path_planner_node.cpp | 2 + .../avoidance/avoidance_module.cpp | 99 +++++++++++++- .../src/scene_module/avoidance/debug.cpp | 129 ++++++++++-------- .../include/route_handler/route_handler.hpp | 9 ++ planning/route_handler/src/route_handler.cpp | 11 ++ 9 files changed, 225 insertions(+), 61 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 93a1e498a757a..26f5b823d9f55 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml @@ -11,11 +11,19 @@ enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false - threshold_distance_object_is_on_center: 1.0 # [m] + # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] threshold_time_object_is_moving: 1.0 # [s] + object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + + threshold_distance_object_is_on_center: 1.0 # [m] + + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + + # For lateral margin object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5cbd4301ea197..591e2991d6f92 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,10 +11,20 @@ enable_avoidance_over_opposite_direction: true enable_update_path_when_object_is_gone: false - threshold_distance_object_is_on_center: 1.0 # [m] + # For target object filtering threshold_speed_object_is_stopped: 1.0 # [m/s] + threshold_time_object_is_moving: 1.0 # [s] + object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + + threshold_distance_object_is_on_center: 1.0 # [m] + + object_check_shiftable_ratio: 0.6 # [-] + object_check_min_road_shoulder_width: 0.5 # [m] + + # For lateral margin + object_envelope_buffer: 0.3 # [m] lateral_collision_margin: 1.0 # [m] lateral_collision_safety_buffer: 0.7 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 50fba4078d194..5968e8f344ca2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -35,6 +35,7 @@ namespace behavior_path_planner using autoware_auto_perception_msgs::msg::PredictedObject; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; @@ -80,6 +81,12 @@ struct AvoidanceParameters // continue to detect backward vehicles as avoidance targets until they are this distance away double object_check_backward_distance; + // use in judge whether the vehicle is parking object on road shoulder + double object_check_shiftable_ratio; + + // minimum road shoulder width. maybe 0.5 [m] + double object_check_min_road_shoulder_width; + // object's enveloped polygon double object_envelope_buffer; @@ -201,6 +208,9 @@ struct ObjectData // avoidance target // lateral distance to the closest footprint, in Frenet coordinate double overhang_dist; + // lateral shiftable ratio + double shiftable_ratio{0.0}; + // count up when object disappeared. Removed when it exceeds threshold. rclcpp::Time last_seen; double lost_time{0.0}; @@ -218,6 +228,9 @@ struct ObjectData // avoidance target // envelope polygon Polygon2d envelope_poly{}; + // envelope polygon centroid + Point2d centroid{}; + // lateral distance from overhang to the road shoulder double to_road_shoulder_distance{0.0}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp index ca2f806a493ab..60e31b0a3a416 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp @@ -37,6 +37,7 @@ namespace marker_utils::avoidance_marker using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidLineArray; +using behavior_path_planner::ObjectDataArray; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; 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 8a72a8da9061c..c0240a43644f3 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -285,6 +285,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0); p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); + p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0); + p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5); p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); p.lateral_collision_margin = dp("lateral_collision_margin", 2.0); p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5); 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 79c692d1de4ad..e27bfd22c7997 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 @@ -191,7 +191,10 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb void AvoidanceModule::fillAvoidanceTargetObjects( AvoidancePlanningData & data, DebugData & debug) const { + using boost::geometry::return_centroid; + using boost::geometry::within; using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; using lanelet::utils::getId; using lanelet::utils::to2D; @@ -224,7 +227,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( std::vector avoidance_debug_msg_array; for (const auto & i : lane_filtered_objects_index) { const auto & object = planner_data_->dynamic_object->objects.at(i); - const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; AvoidanceDebugMsg avoidance_debug_msg; const auto avoidance_debug_array_false_and_push_back = [&avoidance_debug_msg, &avoidance_debug_msg_array](const std::string & failed_reason) { @@ -242,12 +245,15 @@ void AvoidanceModule::fillAvoidanceTargetObjects( object_data.object = object; avoidance_debug_msg.object_id = getUuidStr(object_data); - const auto object_closest_index = findNearestIndex(path_points, object_pos); + const auto object_closest_index = findNearestIndex(path_points, object_pose.position); const auto object_closest_pose = path_points.at(object_closest_index).point.pose; // Calc envelop polygon. fillObjectEnvelopePolygon(object_closest_pose, object_data); + // calc object centroid. + object_data.centroid = return_centroid(object_data.envelope_poly); + // calc longitudinal distance from ego to closest target object footprint point. fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path, ego_pos, object_data); avoidance_debug_msg.longitudinal_distance = object_data.longitudinal; @@ -256,7 +262,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( fillObjectMovingTime(object_data); if (object_data.move_time > parameters_->threshold_time_object_is_moving) { - avoidance_debug_array_false_and_push_back("MovingObject"); + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::MOVING_OBJECT); data.other_objects.push_back(object_data); continue; } @@ -281,7 +287,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( } // Calc lateral deviation from path to target object. - object_data.lateral = calcLateralDeviation(object_closest_pose, object_pos); + object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral; // Find the footprint point closest to the path, set to object_data.overhang_distance. @@ -332,6 +338,89 @@ void AvoidanceModule::fillAvoidanceTargetObjects( continue; } + lanelet::ConstLanelet object_closest_lanelet; + const auto lanelet_map = rh->getLaneletMapPtr(); + if (!lanelet::utils::query::getClosestLanelet( + lanelet::utils::query::laneletLayer(lanelet_map), object_pose, &object_closest_lanelet)) { + continue; + } + + lanelet::BasicPoint2d object_centroid(object_data.centroid.x(), object_data.centroid.y()); + + /** + * Is not object in adjacent lane? + * - Yes -> Is parking object? + * - Yes -> the object is avoidance target. + * - No -> ignore this object. + * - No -> the object is avoidance target no matter whether it is parking object or not. + */ + const auto is_in_ego_lane = + within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); + if (is_in_ego_lane) { + /** + * TODO(Satoshi Ota) use intersection area + * under the assumption that there is no parking vehicle inside intersection, + * ignore all objects that is in the ego lane as not parking objects. + */ + std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + data.other_objects.push_back(object_data); + continue; + } + + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object_closest_lanelet, object_pose.position); + lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + const auto most_left_road_lanelet = rh->getMostLeftLanelet(object_closest_lanelet); + const auto most_left_lanelet_candidates = + rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + + const auto center_to_left_boundary = + distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + double object_shiftable_distance = center_to_left_boundary - 0.5 * object.shape.dimensions.y; + + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters_->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object_closest_lanelet.centerline().basicLineString()), object_centroid); + object_data.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + + const auto is_parking_object = + object_data.shiftable_ratio > parameters_->object_check_shiftable_ratio; + + if (!is_parking_object) { + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::NOT_PARKING_OBJECT); + data.other_objects.push_back(object_data); + continue; + } + } + object_data.last_seen = clock_->now(); // set data @@ -615,7 +704,7 @@ AvoidLineArray AvoidanceModule::calcRawShiftLinesFromObjects(const ObjectDataArr const auto is_object_on_right = isOnRight(o); const auto shift_length = getShiftLength(o, is_object_on_right, avoid_margin); if (isSameDirectionShift(is_object_on_right, shift_length)) { - avoidance_debug_array_false_and_push_back("IgnoreSameDirectionShift"); + avoidance_debug_array_false_and_push_back(AvoidanceDebugFactor::SAME_DIRECTION_SHIFT); continue; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp index 5bfbf7ab8498b..c6ded01021c33 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/debug.cpp @@ -24,14 +24,72 @@ namespace marker_utils::avoidance_marker { + using behavior_path_planner::AvoidLine; using behavior_path_planner::util::shiftPose; +using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; +namespace +{ + +int32_t uuidToInt32(const unique_identifier_msgs::msg::UUID & uuid) +{ + int32_t ret = 0; + + for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { + ret <<= sizeof(int8_t); + ret |= uuid.uuid.at(i); + } + + return ret; +} + +MarkerArray createObjectsCubeMarkerArray( + const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, + const ColorRGBA & color) +{ + MarkerArray msg; + + auto marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, scale, color); + for (const auto & object : objects) { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + msg.markers.push_back(marker); + } + + return msg; +} + +MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns) +{ + MarkerArray msg; + + Marker marker = createDefaultMarker( + "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0)); + + for (const auto & object : objects) { + marker.id = uuidToInt32(object.object.object_id); + marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; + std::ostringstream string_stream; + string_stream << std::fixed << std::setprecision(2); + string_stream << "ratio:" << object.shiftable_ratio << " [-]\n" + << "lateral: " << object.lateral << " [-]"; + marker.text = string_stream.str(); + msg.markers.push_back(marker); + } + + return msg; +} + +} // namespace + MarkerArray createAvoidLineMarkerArray( const AvoidLineArray & shift_lines, std::string && ns, const float & r, const float & g, const float & b, const double & w) @@ -85,35 +143,16 @@ MarkerArray createAvoidLineMarkerArray( MarkerArray createTargetObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { - const auto normal_color = tier4_autoware_utils::createMarkerColor(0.9, 0.0, 0.0, 0.8); - const auto disappearing_color = tier4_autoware_utils::createMarkerColor(0.9, 0.5, 0.9, 0.6); - - const auto uuid_to_id = [](const unique_identifier_msgs::msg::UUID & object_id) { - int32_t ret = 0; - - for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); - ret |= object_id.uuid.at(i); - } - - return ret; - }; - MarkerArray msg; - msg.markers.reserve(objects.size() * 2); + msg.markers.reserve(objects.size() * 3); - { - auto marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), normal_color); - for (const auto & object : objects) { - marker.id = uuid_to_id(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); - marker.color = std::fabs(object.lost_time) < 1e-2 ? normal_color : disappearing_color; - msg.markers.push_back(marker); - } - } + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(1.0, 0.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); { for (const auto & object : objects) { @@ -130,7 +169,7 @@ MarkerArray createTargetObjectsMarkerArray( } marker.points.push_back(marker.points.front()); - marker.id = uuid_to_id(object.object.object_id); + marker.id = uuidToInt32(object.object.object_id); msg.markers.push_back(marker); } } @@ -142,34 +181,16 @@ MarkerArray createTargetObjectsMarkerArray( MarkerArray createOtherObjectsMarkerArray( const behavior_path_planner::ObjectDataArray & objects, std::string && ns) { - const auto normal_color = tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.8); - - const auto uuid_to_id = [](const unique_identifier_msgs::msg::UUID & object_id) { - int32_t ret = 0; - - for (size_t i = 0; i < sizeof(int32_t) / sizeof(int8_t); ++i) { - ret <<= sizeof(int8_t); - ret |= object_id.uuid.at(i); - } - - return ret; - }; - MarkerArray msg; - msg.markers.reserve(objects.size()); + msg.markers.reserve(objects.size() * 2); - { - Marker marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::CUBE, - createMarkerScale(3.0, 1.5, 1.5), normal_color); - for (const auto & object : objects) { - marker.id = uuid_to_id(object.object.object_id); - marker.pose = object.object.kinematics.initial_pose_with_covariance.pose; - marker.scale = tier4_autoware_utils::createMarkerScale(3.0, 1.5, 1.5); - marker.color = normal_color; - msg.markers.push_back(marker); - } - } + appendMarkerArray( + createObjectsCubeMarkerArray( + objects, ns + "_cube", createMarkerScale(3.0, 1.5, 1.5), + createMarkerColor(0.0, 1.0, 0.0, 0.8)), + &msg); + + appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg); return msg; } diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index b359e9dbd8753..59cfe17c88731 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -176,6 +176,15 @@ class RouteHandler const lanelet::ConstLanelet & current_lane, bool is_right = true, bool is_left = true, bool is_opposite = true, const bool & invert_opposite = false) const noexcept; + /** + * @brief Check if same-direction lane is available at the left side of the lanelet + * Searches for any lanes regardless of whether it is lane-changeable or not. + * Required the linestring to be shared(same line ID) between the lanelets. + * @param the lanelet of interest + * @return vector of lanelet having same direction if true + */ + lanelet::ConstLanelet getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const; + /** * @brief Searches the furthest linestring to the right side of the lanelet * Only lanelet with same direction is considered diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 85d34dd45fac5..f3bce09fa8615 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -999,6 +999,17 @@ lanelet::Lanelets RouteHandler::getLeftOppositeLanelets(const lanelet::ConstLane return opposite_lanelets; } +lanelet::ConstLanelet RouteHandler::getMostLeftLanelet(const lanelet::ConstLanelet & lanelet) const +{ + // recursively compute the width of the lanes + const auto & same = getLeftLanelet(lanelet); + + if (same) { + return getMostLeftLanelet(same.get()); + } + return lanelet; +} + lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( const lanelet::ConstLanelet & lanelet) const noexcept { From d6638ee1a2a44e94fa554c29344232c6abb67ee8 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 2 Dec 2022 17:27:38 +0900 Subject: [PATCH 13/13] chore(tier4_state_rviz_plugin): add maintainer (#2435) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- common/tier4_state_rviz_plugin/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/common/tier4_state_rviz_plugin/package.xml b/common/tier4_state_rviz_plugin/package.xml index 74f7a7418815e..b52f7d6cda1ca 100644 --- a/common/tier4_state_rviz_plugin/package.xml +++ b/common/tier4_state_rviz_plugin/package.xml @@ -5,6 +5,7 @@ 0.0.0 The autoware state rviz plugin package Hiroki OTA + Takagi, Isamu Apache License 2.0 ament_cmake_auto