diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 7a9eb25703aaf..51ccec2fbb26b 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -56,7 +56,7 @@ common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomo common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** kotaro.uetake@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +control/autonomous_emergency_braking/** daniel.sanchez@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -69,6 +69,7 @@ control/pid_longitudinal_controller/** mamoru.sobue@tier4.jp takamasa.horibe@tie control/predicted_path_checker/** berkay@leodrive.ai control/pure_pursuit/** takamasa.horibe@tier4.jp control/shift_decider/** takamasa.horibe@tier4.jp +control/smart_mpc_trajectory_follower/** masayuki.aino@proxima-ai-tech.com control/trajectory_follower_base/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/trajectory_follower_node/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp control/vehicle_cmd_gate/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp @@ -156,6 +157,7 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -183,7 +185,7 @@ planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kur planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp -planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -198,7 +200,7 @@ planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.j planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp -planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp @@ -225,7 +227,7 @@ sensing/vehicle_velocity_converter/** ryu.yamamoto@tier4.jp simulator/dummy_perception_publisher/** yukihiro.saito@tier4.jp simulator/fault_injection/** keisuke.shima@tier4.jp simulator/learning_based_vehicle_model/** maxime.clement@tier4.jp nagy.tomas@tier4.jp -simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp +simulator/simple_planning_simulator/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp takamasa.horibe@tier4.jp temkei.kem@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp simulator/vehicle_door_simulator/** isamu.takagi@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp @@ -249,6 +251,7 @@ system/system_error_monitor/** fumihito.ito@tier4.jp system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp +tools/reaction_analyzer/** berkay@leodrive.ai vehicle/accel_brake_map_calibrator/** taiki.tanaka@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp vehicle/external_cmd_converter/** takamasa.horibe@tier4.jp vehicle/raw_vehicle_cmd_converter/** makoto.kurihara@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp diff --git a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp index 8bb5f13e3fb78..fb151ec56b085 100644 --- a/common/motion_utils/include/motion_utils/resample/resample_utils.hpp +++ b/common/motion_utils/include/motion_utils/resample/resample_utils.hpp @@ -27,7 +27,11 @@ namespace resample_utils { constexpr double close_s_threshold = 1e-6; -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"motion_utils.resample_utils"}; + return rclcpp::get_logger(logger); +} template bool validate_size(const T & points) @@ -62,27 +66,27 @@ bool validate_arguments(const T & input_points, const std::vector & resa { // Check size of the arguments if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } if (!validate_size(resampling_intervals)) { - log_error( - "[resample_utils] invalid argument: The number of resampling intervals is less than 2"); + RCLCPP_DEBUG( + get_logger(), "invalid argument: The number of resampling intervals is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } // Check resampling range if (!validate_resampling_range(input_points, resampling_intervals)) { - log_error("[resample_utils] invalid argument: resampling interval is longer than input points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: resampling interval is longer than input points"); tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); tier4_autoware_utils::print_backtrace(); return false; } @@ -95,23 +99,23 @@ bool validate_arguments(const T & input_points, const double resampling_interval { // Check size of the arguments if (!validate_size(input_points)) { - log_error("[resample_utils] invalid argument: The number of input points is less than 2"); + RCLCPP_DEBUG(get_logger(), "invalid argument: The number of input points is less than 2"); tier4_autoware_utils::print_backtrace(); return false; } // check resampling interval if (resampling_interval < motion_utils::overlap_threshold) { - log_error( - "[resample_utils] invalid argument: resampling interval is less than " + - std::to_string(motion_utils::overlap_threshold)); + RCLCPP_DEBUG( + get_logger(), "invalid argument: resampling interval is less than %f", + motion_utils::overlap_threshold); tier4_autoware_utils::print_backtrace(); return false; } // Check duplication if (!validate_points_duplication(input_points)) { - log_error("[resample_utils] invalid argument: input points has some duplicated points"); + RCLCPP_DEBUG(get_logger(), "invalid argument: input points has some duplicated points"); tier4_autoware_utils::print_backtrace(); return false; } diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 7c9b5a5378ab6..d5413cdb8ac24 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -21,6 +21,7 @@ #include "tier4_autoware_utils/system/backtrace.hpp" #include +#include #include #include @@ -33,9 +34,14 @@ #include #include #include + namespace motion_utils { -#define log_error(message) std::cerr << "\033[31m " << message << " \033[0m" << std::endl; +static inline rclcpp::Logger get_logger() +{ + constexpr const char * logger{"motion_utils.trajectory"}; + return rclcpp::get_logger(logger); +} /** * @brief validate if points container is empty or not @@ -216,7 +222,7 @@ std::optional searchZeroVelocityIndex( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -248,7 +254,7 @@ std::optional searchZeroVelocityIndex(const T & points_with_twist, const try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -338,7 +344,7 @@ std::optional findNearestIndex( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -411,9 +417,10 @@ double calcLongitudinalOffsetToSegment( if (throw_exception) { throw std::out_of_range(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -425,7 +432,7 @@ double calcLongitudinalOffsetToSegment( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return std::nan(""); } } @@ -438,9 +445,10 @@ double calcLongitudinalOffsetToSegment( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -586,9 +594,10 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); return std::nan(""); } } @@ -601,9 +610,10 @@ double calcLateralOffset( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -658,9 +668,10 @@ double calcLateralOffset( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error( - std::string(e.what()) + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + e.what()); return std::nan(""); } } @@ -673,9 +684,10 @@ double calcLateralOffset( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return std::nan(""); } @@ -711,7 +723,7 @@ double calcSignedArcLength(const T & points, const size_t src_idx, const size_t try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -754,7 +766,7 @@ std::vector calcSignedArcLengthPartialSum( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -806,7 +818,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -849,7 +861,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -888,7 +900,7 @@ double calcSignedArcLength( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -928,7 +940,7 @@ double calcArcLength(const T & points) try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } @@ -1037,7 +1049,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1071,7 +1083,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1084,9 +1096,10 @@ std::optional calcLongitudinalOffsetPoint( if (throw_exception) { throw std::out_of_range(error_message); } - log_error( - error_message + - " Return NaN since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return NaN since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return {}; } @@ -1153,7 +1166,7 @@ std::optional calcLongitudinalOffsetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); return {}; } @@ -1201,7 +1214,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error("Failed to calculate longitudinal offset: " + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: %s", e.what()); return {}; } @@ -1214,12 +1227,12 @@ std::optional calcLongitudinalOffsetPose( if (throw_exception) { throw std::out_of_range(error_message); } - log_error(error_message); + RCLCPP_DEBUG(get_logger(), "%s", error_message.c_str()); return {}; } if (points.size() == 1) { - log_error("Failed to calculate longitudinal offset: points size is one."); + RCLCPP_DEBUG(get_logger(), "Failed to calculate longitudinal offset: points size is one."); return {}; } @@ -1304,7 +1317,7 @@ std::optional calcLongitudinalOffsetPose( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1350,7 +1363,7 @@ std::optional insertTargetPoint( try { validateNonEmpty(points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -1365,7 +1378,7 @@ std::optional insertTargetPoint( try { validateNonSharpAngle(p_front, p_target, p_back); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return {}; } @@ -2231,7 +2244,7 @@ std::optional calcDistanceToForwardStopPoint( try { validateNonEmpty(points_with_twist); } catch (const std::exception & e) { - log_error("Failed to calculate stop distance" + std::string(e.what())); + RCLCPP_DEBUG(get_logger(), "Failed to calculate stop distance %s", e.what()); return {}; } @@ -2370,7 +2383,7 @@ T cropPoints( cropped_forward_points, target_pos, modified_target_seg_idx, backward_length); if (cropped_points.size() < 2) { - log_error("Return original points since cropped_points size is less than 2."); + RCLCPP_DEBUG(get_logger(), "Return original points since cropped_points size is less than 2."); return points; } @@ -2415,7 +2428,7 @@ double calcYawDeviation( try { validateNonEmpty(overlap_removed_points); } catch (const std::exception & e) { - log_error(e.what()); + RCLCPP_DEBUG(get_logger(), "%s", e.what()); return 0.0; } } @@ -2428,9 +2441,10 @@ double calcYawDeviation( if (throw_exception) { throw std::runtime_error(error_message); } - log_error( - error_message + - " Return 0 since no_throw option is enabled. The maintainer must check the code."); + RCLCPP_DEBUG( + get_logger(), + "%s Return 0 since no_throw option is enabled. The maintainer must check the code.", + error_message.c_str()); return 0.0; } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index eb6a06041e65d..3fe6e6851b45e 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -1876,40 +1876,34 @@ TEST(trajectory, insertTargetPoint) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle"), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -2304,13 +2298,11 @@ TEST(trajectory, insertTargetPoint_Length) // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } @@ -4310,40 +4302,34 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) // Invalid target point(In front of begin point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(-1.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(10.0, 0.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) { - testing::internal::CaptureStderr(); auto traj_out = traj; const auto p_target = createPoint(4.0, 10.0, 0.0); const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(testing::internal::GetCapturedStderr().find("sharp angle."), std::string::npos); EXPECT_EQ(insert_idx, std::nullopt); } diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp index f8d5baaf02777..b842261d56cfa 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/polling_subscriber.hpp @@ -17,6 +17,8 @@ #include +#include +#include #include namespace tier4_autoware_utils @@ -27,10 +29,11 @@ class InterProcessPollingSubscriber { private: typename rclcpp::Subscription::SharedPtr subscriber_; - std::optional data_; + typename T::SharedPtr data_; public: - explicit InterProcessPollingSubscriber(rclcpp::Node * node, const std::string & topic_name) + explicit InterProcessPollingSubscriber( + rclcpp::Node * node, const std::string & topic_name, const rclcpp::QoS & qos = rclcpp::QoS{1}) { auto noexec_callback_group = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -38,26 +41,25 @@ class InterProcessPollingSubscriber noexec_subscription_options.callback_group = noexec_callback_group; subscriber_ = node->create_subscription( - topic_name, rclcpp::QoS{1}, + topic_name, qos, [node]([[maybe_unused]] const typename T::ConstSharedPtr msg) { assert(false); }, noexec_subscription_options); - }; - bool updateLatestData() - { - rclcpp::MessageInfo message_info; - T tmp; - // The queue size (QoS) must be 1 to get the last message data. - if (subscriber_->take(tmp, message_info)) { - data_ = tmp; + if (qos.get_rmw_qos_profile().depth > 1) { + throw std::invalid_argument( + "InterProcessPollingSubscriber will be used with depth > 1, which may cause inefficient " + "serialization while updateLatestData()"); } - return data_.has_value(); }; - const T & getData() const + typename T::ConstSharedPtr takeData() { - if (!data_.has_value()) { - throw std::runtime_error("Bad_optional_access in class InterProcessPollingSubscriber"); + auto new_data = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = subscriber_->take(*new_data, message_info); + if (success) { + data_ = new_data; } - return data_.value(); + + return data_; }; }; diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 6b1214b133af4..e8d2f775e9273 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -35,6 +35,8 @@ Planning: logger_name: tier4_autoware_utils - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner logger_name: behavior_path_planner.path_shifter + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: motion_utils behavior_path_planner_avoidance: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner @@ -67,6 +69,8 @@ Planning: logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_velocity_planner - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: motion_utils behavior_velocity_planner_intersection: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner @@ -77,12 +81,16 @@ Planning: logger_name: planning.scenario_planning.lane_driving.motion_planning.obstacle_avoidance_planner - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: motion_utils motion_velocity_smoother: - node_name: /planning/scenario_planning/motion_velocity_smoother logger_name: planning.scenario_planning.motion_velocity_smoother - node_name: /planning/scenario_planning/motion_velocity_smoother logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: motion_utils route_handler: - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner @@ -97,12 +105,16 @@ Control: logger_name: control.trajectory_follower.controller_node_exe.lateral_controller - node_name: /control/trajectory_follower/controller_node_exe logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: motion_utils longitudinal_controller: - node_name: /control/trajectory_follower/controller_node_exe logger_name: control.trajectory_follower.controller_node_exe.longitudinal_controller - node_name: /control/trajectory_follower/controller_node_exe logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: motion_utils vehicle_cmd_gate: - node_name: /control/vehicle_cmd_gate diff --git a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp index 43fb310b17416..27f04603d6a31 100644 --- a/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp +++ b/control/autonomous_emergency_braking/include/autonomous_emergency_braking/node.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -224,18 +225,28 @@ class CollisionDataKeeper rclcpp::Clock::SharedPtr clock_; }; +static rclcpp::SensorDataQoS SingleDepthSensorQoS() +{ + rclcpp::SensorDataQoS qos; + qos.get_rmw_qos_profile().depth = 1; + return qos; +} + class AEB : public rclcpp::Node { public: explicit AEB(const rclcpp::NodeOptions & node_options); // subscriber - rclcpp::Subscription::SharedPtr sub_point_cloud_; - rclcpp::Subscription::SharedPtr sub_velocity_; - rclcpp::Subscription::SharedPtr sub_imu_; - rclcpp::Subscription::SharedPtr sub_predicted_traj_; - rclcpp::Subscription::SharedPtr sub_autoware_state_; - + tier4_autoware_utils::InterProcessPollingSubscriber sub_point_cloud_{ + this, "~/input/pointcloud", SingleDepthSensorQoS()}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_velocity_{ + this, "~/input/velocity"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_imu_{this, "~/input/imu"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_predicted_traj_{ + this, "~/input/predicted_trajectory"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_autoware_state_{ + this, "/autoware/state"}; // publisher rclcpp::Publisher::SharedPtr pub_obstacle_pointcloud_; rclcpp::Publisher::SharedPtr debug_ego_path_publisher_; // debug @@ -245,15 +256,12 @@ class AEB : public rclcpp::Node // callback void onPointCloud(const PointCloud2::ConstSharedPtr input_msg); - void onVelocity(const VelocityReport::ConstSharedPtr input_msg); void onImu(const Imu::ConstSharedPtr input_msg); void onTimer(); - void onPredictedTrajectory(const Trajectory::ConstSharedPtr input_msg); - void onAutowareState(const AutowareState::ConstSharedPtr input_msg); rcl_interfaces::msg::SetParametersResult onParameter( const std::vector & parameters); - bool isDataReady(); + bool fetchLatestData(); // main function void onCheckCollision(DiagnosticStatusWrapper & stat); diff --git a/control/autonomous_emergency_braking/package.xml b/control/autonomous_emergency_braking/package.xml index 1ac255c21921b..68c070a86dd97 100644 --- a/control/autonomous_emergency_braking/package.xml +++ b/control/autonomous_emergency_braking/package.xml @@ -7,6 +7,7 @@ Takamasa Horibe Tomoya Kimura Mamoru Sobue + Daniel Sanchez Apache License 2.0 diff --git a/control/autonomous_emergency_braking/src/node.cpp b/control/autonomous_emergency_braking/src/node.cpp index d8886672a8ecd..905b66df288b4 100644 --- a/control/autonomous_emergency_braking/src/node.cpp +++ b/control/autonomous_emergency_braking/src/node.cpp @@ -105,27 +105,6 @@ AEB::AEB(const rclcpp::NodeOptions & node_options) vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()), collision_data_keeper_(this->get_clock()) { - // Subscribers - { - sub_point_cloud_ = this->create_subscription( - "~/input/pointcloud", rclcpp::SensorDataQoS(), - std::bind(&AEB::onPointCloud, this, std::placeholders::_1)); - - sub_velocity_ = this->create_subscription( - "~/input/velocity", rclcpp::QoS{1}, std::bind(&AEB::onVelocity, this, std::placeholders::_1)); - - sub_imu_ = this->create_subscription( - "~/input/imu", rclcpp::QoS{1}, std::bind(&AEB::onImu, this, std::placeholders::_1)); - - sub_predicted_traj_ = this->create_subscription( - "~/input/predicted_trajectory", rclcpp::QoS{1}, - std::bind(&AEB::onPredictedTrajectory, this, std::placeholders::_1)); - - sub_autoware_state_ = this->create_subscription( - "/autoware/state", rclcpp::QoS{1}, - std::bind(&AEB::onAutowareState, this, std::placeholders::_1)); - } - // Publisher { pub_obstacle_pointcloud_ = @@ -229,11 +208,6 @@ void AEB::onTimer() updater_.force_update(); } -void AEB::onVelocity(const VelocityReport::ConstSharedPtr input_msg) -{ - current_velocity_ptr_ = input_msg; -} - void AEB::onImu(const Imu::ConstSharedPtr input_msg) { // transform imu @@ -253,17 +227,6 @@ void AEB::onImu(const Imu::ConstSharedPtr input_msg) tf2::doTransform(input_msg->angular_velocity, *angular_velocity_ptr_, transform_stamped); } -void AEB::onPredictedTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg) -{ - predicted_traj_ptr_ = input_msg; -} - -void AEB::onAutowareState(const AutowareState::ConstSharedPtr input_msg) -{ - autoware_state_ = input_msg; -} - void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) { PointCloud::Ptr pointcloud_ptr(new PointCloud); @@ -316,29 +279,42 @@ void AEB::onPointCloud(const PointCloud2::ConstSharedPtr input_msg) obstacle_ros_pointcloud_ptr_->header = input_msg->header; } -bool AEB::isDataReady() +bool AEB::fetchLatestData() { const auto missing = [this](const auto & name) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), *get_clock(), 5000, "[AEB] waiting for %s", name); return false; }; + current_velocity_ptr_ = sub_velocity_.takeData(); if (!current_velocity_ptr_) { return missing("ego velocity"); } + const auto pointcloud_ptr = sub_point_cloud_.takeData(); + if (!pointcloud_ptr) { + return missing("object pointcloud message"); + } + onPointCloud(pointcloud_ptr); if (!obstacle_ros_pointcloud_ptr_) { return missing("object pointcloud"); } + const auto imu_ptr = sub_imu_.takeData(); + if (use_imu_path_ && !imu_ptr) { + return missing("imu message"); + } + onImu(imu_ptr); if (use_imu_path_ && !angular_velocity_ptr_) { return missing("imu"); } + predicted_traj_ptr_ = sub_predicted_traj_.takeData(); if (use_predicted_trajectory_ && !predicted_traj_ptr_) { return missing("control predicted trajectory"); } + autoware_state_ = sub_autoware_state_.takeData(); if (!autoware_state_) { return missing("autoware_state"); } @@ -375,7 +351,7 @@ bool AEB::checkCollision(MarkerArray & debug_markers) using colorTuple = std::tuple; // step1. check data - if (!isDataReady()) { + if (!fetchLatestData()) { return false; } diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 8e7685180c1f9..8692027e9634a 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -464,17 +464,14 @@ bool NDTScanMatcher::callback_sensor_points_main( diagnostics_scan_points_->addKeyValue("transform_probability", ndt_result.transform_probability); diagnostics_scan_points_->addKeyValue( "nearest_voxel_transformation_likelihood", ndt_result.nearest_voxel_transformation_likelihood); - std::string score_name = ""; double score = 0.0; double score_threshold = 0.0; if (param_.score_estimation.converged_param_type == ConvergedParamType::TRANSFORM_PROBABILITY) { - score_name = "Transform Probability"; score = ndt_result.transform_probability; score_threshold = param_.score_estimation.converged_param_transform_probability; } else if ( param_.score_estimation.converged_param_type == ConvergedParamType::NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD) { - score_name = "Nearest Voxel Transformation Likelihood"; score = ndt_result.nearest_voxel_transformation_likelihood; score_threshold = param_.score_estimation.converged_param_nearest_voxel_transformation_likelihood; diff --git a/map/map_height_fitter/src/map_height_fitter.cpp b/map/map_height_fitter/src/map_height_fitter.cpp index 095574125d9a0..0c99d33772aea 100644 --- a/map/map_height_fitter/src/map_height_fitter.cpp +++ b/map/map_height_fitter/src/map_height_fitter.cpp @@ -200,20 +200,12 @@ double MapHeightFitter::Impl::get_ground_height(const Point & point) const } } } else if (fit_target_ == "vector_map") { - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(vector_map_); - - geometry_msgs::msg::Pose pose; - pose.position.x = x; - pose.position.y = y; - pose.position.z = 0.0; - lanelet::ConstLanelet closest_lanelet; - const bool result = - lanelet::utils::query::getClosestLanelet(all_lanelets, pose, &closest_lanelet); - if (!result) { + const auto closest_points = vector_map_->pointLayer.nearest(lanelet::BasicPoint2d{x, y}, 1); + if (closest_points.empty()) { RCLCPP_WARN_STREAM(logger, "failed to get closest lanelet"); return point.z; } - height = closest_lanelet.centerline().back().z(); + height = closest_points.front().z(); } return std::isfinite(height) ? height : point.z; diff --git a/map/map_projection_loader/README.md b/map/map_projection_loader/README.md index 848fcfba95f14..aa620256d977b 100644 --- a/map/map_projection_loader/README.md +++ b/map/map_projection_loader/README.md @@ -21,6 +21,10 @@ sample-map-rosbag └── pointcloud_map_metadata.yaml ``` +There are three types of transformations from latitude and longitude to XYZ coordinate system as shown in the figure below. Please refer to the following details for the necessary parameters for each projector type. + +![node_diagram](docs/map_projector_type.svg) + ### Using local coordinate ```yaml diff --git a/map/map_projection_loader/docs/map_projector_type.svg b/map/map_projection_loader/docs/map_projector_type.svg new file mode 100644 index 0000000000000..e1c8c2ac68987 --- /dev/null +++ b/map/map_projection_loader/docs/map_projector_type.svg @@ -0,0 +1,3010 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + MGRS + + + X(east) + + + O + + + Y(north) + + + + Strict Boundary. + + Outside of the mgrs grid is + projected to undefined xy + + + mgrs_grid=54SUE + + + 54SUE + + + 54S(UTM grid) + + + meridian + + + LocalCartesianUTM + + + TransverseMercator + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + 3+6*floor(map_origin.longitude/6)[deg] + + + 3+6n[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + map_origin.latitude=35.6 + map_origin.longitude=139.5 + + + map_origin.longitude[deg] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + X(east) + + + O + + + Y(north) + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + + No boundary, + but farther away from the meridian, + the greater the projection error becomes + + + diff --git a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp index 8900381060472..a406e5381357d 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_lane_change_tags.cpp @@ -48,11 +48,6 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - lanelet::Lanelets convertToVector(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Lanelets lanelets; diff --git a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp index b0eca472ee1f3..758fee3addc06 100644 --- a/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp +++ b/map/util/lanelet2_map_preprocessor/src/fix_z_value_by_pcd.cpp @@ -91,11 +91,6 @@ double getMinHeightAroundPoint( return min_height; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - void adjustHeight( const pcl::PointCloud::Ptr & pcd_map_ptr, lanelet::LaneletMapPtr & lanelet_map_ptr) { @@ -108,7 +103,7 @@ void adjustHeight( for (lanelet::Lanelet & llt : lanelet_map_ptr->laneletLayer) { for (lanelet::Point3d & pt : llt.leftBound()) { - if (exists(done, pt.id())) { + if (done.find(pt.id()) != done.end()) { continue; } pcl::PointXYZ pcl_pt; @@ -122,7 +117,7 @@ void adjustHeight( done.insert(pt.id()); } for (lanelet::Point3d & pt : llt.rightBound()) { - if (exists(done, pt.id())) { + if (done.find(pt.id()) != done.end()) { continue; } pcl::PointXYZ pcl_pt; diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp index a9e45b3b31785..d001bdc54a680 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_lines.cpp @@ -49,7 +49,7 @@ bool loadLaneletMap( bool exists(std::unordered_set & set, lanelet::Id element) { - return std::find(set.begin(), set.end(), element) != set.end(); + return set.find(element) != set.end(); } lanelet::LineStrings3d convertLineLayerToLineStrings(lanelet::LaneletMapPtr & lanelet_map_ptr) diff --git a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp index ab77b18493120..beb736e809275 100644 --- a/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp +++ b/map/util/lanelet2_map_preprocessor/src/merge_close_points.cpp @@ -45,7 +45,7 @@ bool loadLaneletMap( bool exists(std::unordered_set & set, lanelet::Id element) { - return std::find(set.begin(), set.end(), element) != set.end(); + return set.find(element) != set.end(); } lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) diff --git a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp index ca488b3acb790..e6c4feb4cee9a 100644 --- a/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp +++ b/map/util/lanelet2_map_preprocessor/src/remove_unreferenced_geometry.cpp @@ -43,11 +43,6 @@ bool loadLaneletMap( return true; } -bool exists(std::unordered_set & set, lanelet::Id element) -{ - return std::find(set.begin(), set.end(), element) != set.end(); -} - lanelet::Points3d convertPointsLayerToPoints(lanelet::LaneletMapPtr & lanelet_map_ptr) { lanelet::Points3d points; diff --git a/perception/lidar_centerpoint/CMakeLists.txt b/perception/lidar_centerpoint/CMakeLists.txt index 8fab65ef6b4d7..353f797b0bee1 100644 --- a/perception/lidar_centerpoint/CMakeLists.txt +++ b/perception/lidar_centerpoint/CMakeLists.txt @@ -147,6 +147,20 @@ if(TRT_AVAIL AND CUDA_AVAIL AND CUDNN_AVAIL) TARGETS centerpoint_cuda_lib DESTINATION lib ) + + if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_detection_class_remapper + test/test_detection_class_remapper.cpp + ) + ament_auto_add_gtest(test_ros_utils + test/test_ros_utils.cpp + ) + ament_auto_add_gtest(test_nms + test/test_nms.cpp + ) + endif() + else() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() diff --git a/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp b/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp new file mode 100644 index 0000000000000..6559736ede198 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_detection_class_remapper.cpp @@ -0,0 +1,92 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +TEST(DetectionClassRemapperTest, MapClasses) +{ + centerpoint::DetectionClassRemapper remapper; + + // Set up the parameters for the remapper + // Labels: CAR, TRUCK, TRAILER + std::vector allow_remapping_by_area_matrix = { + 0, 0, 0, // CAR cannot be remapped + 0, 0, 1, // TRUCK can be remapped to TRAILER + 0, 1, 0 // TRAILER can be remapped to TRUCK + }; + std::vector min_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 10.0, 0.0, 0.0, 0.0}; + std::vector max_area_matrix = {0.0, 0.0, 0.0, 0.0, 0.0, 999.0, 0.0, 10.0, 0.0}; + + remapper.setParameters(allow_remapping_by_area_matrix, min_area_matrix, max_area_matrix); + + // Create a DetectedObjects message with some objects + autoware_auto_perception_msgs::msg::DetectedObjects msg; + + // CAR with area 4.0, which is out of the range for remapping + autoware_auto_perception_msgs::msg::DetectedObject obj1; + autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification; + obj1.shape.dimensions.x = 2.0; + obj1.shape.dimensions.y = 2.0; + obj1_classification.label = 0; + obj1_classification.probability = 1.0; + obj1.classification = {obj1_classification}; + msg.objects.push_back(obj1); + + // TRUCK with area 16.0, which is in the range for remapping to TRAILER + autoware_auto_perception_msgs::msg::DetectedObject obj2; + autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification; + obj2.shape.dimensions.x = 8.0; + obj2.shape.dimensions.y = 2.0; + obj2_classification.label = 1; + obj2_classification.probability = 1.0; + obj2.classification = {obj2_classification}; + msg.objects.push_back(obj2); + + // TRAILER with area 9.0, which is in the range for remapping to TRUCK + autoware_auto_perception_msgs::msg::DetectedObject obj3; + autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification; + obj3.shape.dimensions.x = 3.0; + obj3.shape.dimensions.y = 3.0; + obj3_classification.label = 2; + obj3_classification.probability = 1.0; + obj3.classification = {obj3_classification}; + msg.objects.push_back(obj3); + + // TRAILER with area 12.0, which is out of the range for remapping + autoware_auto_perception_msgs::msg::DetectedObject obj4; + autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification; + obj4.shape.dimensions.x = 4.0; + obj4.shape.dimensions.y = 3.0; + obj4_classification.label = 2; + obj4_classification.probability = 1.0; + obj4.classification = {obj4_classification}; + msg.objects.push_back(obj4); + + // Call the mapClasses function + remapper.mapClasses(msg); + + // Check the remapped labels + EXPECT_EQ(msg.objects[0].classification[0].label, 0); + EXPECT_EQ(msg.objects[1].classification[0].label, 2); + EXPECT_EQ(msg.objects[2].classification[0].label, 1); + EXPECT_EQ(msg.objects[3].classification[0].label, 2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_nms.cpp b/perception/lidar_centerpoint/test/test_nms.cpp new file mode 100644 index 0000000000000..bf443d8d3ad0f --- /dev/null +++ b/perception/lidar_centerpoint/test/test_nms.cpp @@ -0,0 +1,121 @@ +// Copyright 2024 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 "lidar_centerpoint/postprocess/non_maximum_suppression.hpp" + +#include + +TEST(NonMaximumSuppressionTest, Apply) +{ + centerpoint::NonMaximumSuppression nms; + centerpoint::NMSParams params; + params.search_distance_2d_ = 1.0; + params.iou_threshold_ = 0.2; + params.nms_type_ = centerpoint::NMS_TYPE::IoU_BEV; + params.target_class_names_ = {"CAR"}; + nms.setParameters(params); + + std::vector input_objects(4); + + // Object 1 + autoware_auto_perception_msgs::msg::ObjectClassification obj1_classification; + obj1_classification.label = 0; // Assuming "car" has label 0 + obj1_classification.probability = 1.0; + input_objects[0].classification = {obj1_classification}; // Assuming "car" has label 0 + input_objects[0].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[0].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[0].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[0].shape.dimensions.x = 4.0; + input_objects[0].shape.dimensions.y = 2.0; + + // Object 2 (overlaps with Object 1) + autoware_auto_perception_msgs::msg::ObjectClassification obj2_classification; + obj2_classification.label = 0; // Assuming "car" has label 0 + obj2_classification.probability = 1.0; + input_objects[1].classification = {obj2_classification}; // Assuming "car" has label 0 + input_objects[1].kinematics.pose_with_covariance.pose.position.x = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.position.y = 0.5; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[1].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[1].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[1].shape.dimensions.x = 4.0; + input_objects[1].shape.dimensions.y = 2.0; + + // Object 3 + autoware_auto_perception_msgs::msg::ObjectClassification obj3_classification; + obj3_classification.label = 0; // Assuming "car" has label 0 + obj3_classification.probability = 1.0; + input_objects[2].classification = {obj3_classification}; // Assuming "car" has label 0 + input_objects[2].kinematics.pose_with_covariance.pose.position.x = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.position.y = 5.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[2].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[2].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[2].shape.dimensions.x = 4.0; + input_objects[2].shape.dimensions.y = 2.0; + + // Object 4 (different class) + autoware_auto_perception_msgs::msg::ObjectClassification obj4_classification; + obj4_classification.label = 1; // Assuming "pedestrian" has label 1 + obj4_classification.probability = 1.0; + input_objects[3].classification = {obj4_classification}; // Assuming "pedestrian" has label 1 + input_objects[3].kinematics.pose_with_covariance.pose.position.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.position.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.x = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.y = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.z = 0.0; + input_objects[3].kinematics.pose_with_covariance.pose.orientation.w = 1.0; + input_objects[3].shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + input_objects[3].shape.dimensions.x = 0.5; + input_objects[3].shape.dimensions.y = 0.5; + + std::vector output_objects = nms.apply(input_objects); + + // Assert the expected number of output objects + EXPECT_EQ(output_objects.size(), 3); + + // Assert that input_objects[2] is included in the output_objects + bool is_input_object_2_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[2]) { + is_input_object_2_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_2_included); + + // Assert that input_objects[3] is included in the output_objects + bool is_input_object_3_included = false; + for (const auto & output_object : output_objects) { + if (output_object == input_objects[3]) { + is_input_object_3_included = true; + break; + } + } + EXPECT_TRUE(is_input_object_3_included); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/lidar_centerpoint/test/test_ros_utils.cpp b/perception/lidar_centerpoint/test/test_ros_utils.cpp new file mode 100644 index 0000000000000..8d943bac61123 --- /dev/null +++ b/perception/lidar_centerpoint/test/test_ros_utils.cpp @@ -0,0 +1,141 @@ +// Copyright 2024 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 "lidar_centerpoint/ros_utils.hpp" + +#include + +TEST(TestSuite, box3DToDetectedObject) +{ + std::vector class_names = {"CAR", "TRUCK", "BUS", "TRAILER", + "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; + + // Test case 1: Test with valid label, has_twist=true, has_variance=true + { + centerpoint::Box3D box3d; + box3d.score = 0.8f; + box3d.label = 0; // CAR + box3d.x = 1.0; + box3d.y = 2.0; + box3d.z = 3.0; + box3d.yaw = 0.5; + box3d.length = 4.0; + box3d.width = 2.0; + box3d.height = 1.5; + box3d.vel_x = 1.0; + box3d.vel_y = 0.5; + box3d.x_variance = 0.1; + box3d.y_variance = 0.2; + box3d.z_variance = 0.3; + box3d.yaw_variance = 0.4; + box3d.vel_x_variance = 0.5; + box3d.vel_y_variance = 0.6; + + autoware_auto_perception_msgs::msg::DetectedObject obj; + centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); + EXPECT_EQ( + obj.classification[0].label, autoware_auto_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0); + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0); + EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5); + EXPECT_TRUE(obj.kinematics.has_position_covariance); + EXPECT_TRUE(obj.kinematics.has_twist); + EXPECT_TRUE(obj.kinematics.has_twist_covariance); + } + + // Test case 2: Test with invalid label, has_twist=false, has_variance=false + { + centerpoint::Box3D box3d; + box3d.score = 0.5f; + box3d.label = 10; // Invalid + + autoware_auto_perception_msgs::msg::DetectedObject obj; + centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj); + + EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); + EXPECT_EQ( + obj.classification[0].label, + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN); + EXPECT_FALSE(obj.kinematics.has_position_covariance); + EXPECT_FALSE(obj.kinematics.has_twist); + EXPECT_FALSE(obj.kinematics.has_twist_covariance); + } +} + +TEST(TestSuite, getSemanticType) +{ + EXPECT_EQ( + centerpoint::getSemanticType("CAR"), + autoware_auto_perception_msgs::msg::ObjectClassification::CAR); + EXPECT_EQ( + centerpoint::getSemanticType("TRUCK"), + autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK); + EXPECT_EQ( + centerpoint::getSemanticType("BUS"), + autoware_auto_perception_msgs::msg::ObjectClassification::BUS); + EXPECT_EQ( + centerpoint::getSemanticType("TRAILER"), + autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER); + EXPECT_EQ( + centerpoint::getSemanticType("BICYCLE"), + autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE); + EXPECT_EQ( + centerpoint::getSemanticType("MOTORBIKE"), + autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE); + EXPECT_EQ( + centerpoint::getSemanticType("PEDESTRIAN"), + autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN); + EXPECT_EQ( + centerpoint::getSemanticType("UNKNOWN"), + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN); +} + +TEST(TestSuite, convertPoseCovarianceMatrix) +{ + centerpoint::Box3D box3d; + box3d.x_variance = 0.1; + box3d.y_variance = 0.2; + box3d.z_variance = 0.3; + box3d.yaw_variance = 0.4; + + std::array pose_covariance = centerpoint::convertPoseCovarianceMatrix(box3d); + + EXPECT_FLOAT_EQ(pose_covariance[0], 0.1); + EXPECT_FLOAT_EQ(pose_covariance[7], 0.2); + EXPECT_FLOAT_EQ(pose_covariance[14], 0.3); + EXPECT_FLOAT_EQ(pose_covariance[35], 0.4); +} + +TEST(TestSuite, convertTwistCovarianceMatrix) +{ + centerpoint::Box3D box3d; + box3d.vel_x_variance = 0.1; + box3d.vel_y_variance = 0.2; + + std::array twist_covariance = centerpoint::convertTwistCovarianceMatrix(box3d); + + EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); + EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 9378e09f099cc..786446457143f 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -26,6 +26,22 @@ rclcpp_components_register_node(map_based_prediction_node EXECUTABLE map_based_prediction ) +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) + + target_link_libraries(test_map_based_prediction + map_based_prediction_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE config diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 6dfc4a8db9e20..5023051da5e71 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -84,14 +84,14 @@ class PathGenerator const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity); - PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); + PredictedPath generatePathForNonVehicleObject(const TrackedObject & object) const; PredictedPath generatePathForLowSpeedVehicle(const TrackedObject & object) const; - PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object); + PredictedPath generatePathForOffLaneVehicle(const TrackedObject & object) const; PredictedPath generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit = 0.0) const; PredictedPath generatePathForCrosswalkUser( const TrackedObject & object, const CrosswalkEdgePoints & reachable_crosswalk) const; @@ -122,24 +122,25 @@ class PathGenerator PredictedPath generateStraightPath(const TrackedObject & object) const; PredictedPath generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; FrenetPath generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length); + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const; Eigen::Vector3d calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; Eigen::Vector2d calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T); + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const; PosePath interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path); + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const; PredictedPath convertToPredictedPath( const TrackedObject & object, const FrenetPath & frenet_predicted_path, - const PosePath & ref_path); + const PosePath & ref_path) const; FrenetPoint getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0); + const TrackedObject & object, const PosePath & ref_path, const double speed_limit = 0.0) const; }; } // namespace map_based_prediction diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 83fbc16feb7fa..4238859535c8e 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -32,7 +32,7 @@ PathGenerator::PathGenerator( { } -PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForNonVehicleObject(const TrackedObject & object) const { return generateStraightPath(object); } @@ -143,13 +143,13 @@ PredictedPath PathGenerator::generatePathForLowSpeedVehicle(const TrackedObject return path; } -PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) +PredictedPath PathGenerator::generatePathForOffLaneVehicle(const TrackedObject & object) const { return generateStraightPath(object); } PredictedPath PathGenerator::generatePathForOnLaneVehicle( - const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) + const TrackedObject & object, const PosePath & ref_paths, const double speed_limit) const { if (ref_paths.size() < 2) { return generateStraightPath(object); @@ -178,7 +178,7 @@ PredictedPath PathGenerator::generateStraightPath(const TrackedObject & object) } PredictedPath PathGenerator::generatePolynomialPath( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const { // Get current Frenet Point const double ref_path_len = motion_utils::calcArcLength(ref_path); @@ -210,7 +210,8 @@ PredictedPath PathGenerator::generatePolynomialPath( } FrenetPath PathGenerator::generateFrenetPath( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double max_length) + const FrenetPoint & current_point, const FrenetPoint & target_point, + const double max_length) const { FrenetPath path; const double duration = time_horizon_; @@ -252,7 +253,7 @@ FrenetPath PathGenerator::generateFrenetPath( } Eigen::Vector3d PathGenerator::calcLatCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Lateral Path Calculation // Quintic polynomial for d @@ -278,7 +279,7 @@ Eigen::Vector3d PathGenerator::calcLatCoefficients( } Eigen::Vector2d PathGenerator::calcLonCoefficients( - const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) + const FrenetPoint & current_point, const FrenetPoint & target_point, const double T) const { // Longitudinal Path Calculation // Quadric polynomial @@ -296,7 +297,7 @@ Eigen::Vector2d PathGenerator::calcLonCoefficients( } PosePath PathGenerator::interpolateReferencePath( - const PosePath & base_path, const FrenetPath & frenet_predicted_path) + const PosePath & base_path, const FrenetPath & frenet_predicted_path) const { PosePath interpolated_path; const size_t interpolate_num = frenet_predicted_path.size(); @@ -356,7 +357,8 @@ PosePath PathGenerator::interpolateReferencePath( } PredictedPath PathGenerator::convertToPredictedPath( - const TrackedObject & object, const FrenetPath & frenet_predicted_path, const PosePath & ref_path) + const TrackedObject & object, const FrenetPath & frenet_predicted_path, + const PosePath & ref_path) const { PredictedPath predicted_path; predicted_path.time_step = rclcpp::Duration::from_seconds(sampling_time_interval_); @@ -385,7 +387,7 @@ PredictedPath PathGenerator::convertToPredictedPath( } FrenetPoint PathGenerator::getFrenetPoint( - const TrackedObject & object, const PosePath & ref_path, const double speed_limit) + const TrackedObject & object, const PosePath & ref_path, const double speed_limit) const { FrenetPoint frenet_point; const auto obj_point = object.kinematics.pose_with_covariance.pose.position; diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp new file mode 100644 index 0000000000000..555dc217fb5ed --- /dev/null +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -0,0 +1,218 @@ +// Copyright 2024 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 "map_based_prediction/path_generator.hpp" + +#include +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +TrackedObject generate_static_object(const int label) +{ + ObjectClassification classification; + classification.probability = 1.0; + classification.label = label; + + TrackedObjectKinematics kinematics; + kinematics.pose_with_covariance = geometry_msgs::msg::PoseWithCovariance(); // At origin + kinematics.twist_with_covariance = geometry_msgs::msg::TwistWithCovariance(); // Not moving + kinematics.acceleration_with_covariance = + geometry_msgs::msg::AccelWithCovariance(); // Not accelerating + kinematics.orientation_availability = TrackedObjectKinematics::UNAVAILABLE; + + TrackedObject tracked_object; + tracked_object.object_id = unique_identifier_msgs::msg::UUID(); + tracked_object.existence_probability = 1.0; + tracked_object.classification.push_back(classification); + tracked_object.kinematics = kinematics; + + return tracked_object; +} + +TEST(PathGenerator, test_generatePathForNonVehicleObject) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate pedestrian object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForNonVehicleObject(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForLowSpeedVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForLowSpeedVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOffLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOnLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate reference path + map_based_prediction::PosePath ref_paths; + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + ref_paths.push_back(pose); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForCrosswalkUser) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate dummy crosswalk + map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + reachable_crosswalk.front_center_point << 0.0, 0.0; + reachable_crosswalk.front_right_point << 1.0, 0.0; + reachable_crosswalk.front_left_point << -1.0, 0.0; + reachable_crosswalk.back_center_point << 0.0, 1.0; + reachable_crosswalk.back_right_point << 1.0, 1.0; + reachable_crosswalk.back_left_point << -1.0, 1.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathToTargetPoint) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate target point + Eigen::Vector2d target_point; + target_point << 0.0, 0.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathToTargetPoint(tracked_object, target_point); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp b/perception/map_based_prediction/test/test_map_based_prediction.cpp similarity index 67% rename from system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp rename to perception/map_based_prediction/test/test_map_based_prediction.cpp index 4aaab3296f64b..4c8ad7dd25916 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_node.cpp +++ b/perception/map_based_prediction/test/test_map_based_prediction.cpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2024 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. @@ -12,21 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "mrm_handler/mrm_handler_core.hpp" - #include -#include +#include -int main(int argc, char ** argv) +int main(int argc, char * argv[]) { + testing::InitGoogleTest(&argc, argv); rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); + bool result = RUN_ALL_TESTS(); rclcpp::shutdown(); - - return 0; + return result; } diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 7c1b98c10d14b..ade45847f8a9f 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -13,18 +13,23 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(nlohmann_json REQUIRED) # for debug find_package(glog REQUIRED) +find_package(ament_cmake_gtest REQUIRED) include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} ) +ament_auto_add_library(radar_object_tracker_utils SHARED + src/utils/utils.cpp + src/utils/radar_object_tracker_utils.cpp +) + ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp src/tracker/model/linear_motion_tracker.cpp src/tracker/model/constant_turn_rate_motion_tracker.cpp - src/utils/utils.cpp src/data_association/data_association.cpp src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) @@ -34,6 +39,7 @@ target_link_libraries(radar_object_tracker_node yaml-cpp nlohmann_json::nlohmann_json # for debug glog::glog + radar_object_tracker_utils ) rclcpp_components_register_node(radar_object_tracker_node @@ -42,7 +48,21 @@ rclcpp_components_register_node(radar_object_tracker_node ) # testing -# todo +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_radar_object_tracker_utils + test/test_radar_object_tracker_utils.cpp + src/utils/radar_object_tracker_utils.cpp + ) + target_include_directories(test_radar_object_tracker_utils PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + target_link_libraries(test_radar_object_tracker_utils + radar_object_tracker_utils + ) +endif() # Package ament_auto_package( diff --git a/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp new file mode 100644 index 0000000000000..48f40f62e3963 --- /dev/null +++ b/perception/radar_object_tracker/include/radar_object_tracker/utils/radar_object_tracker_utils.hpp @@ -0,0 +1,77 @@ +// Copyright 2024 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_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ +#define RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ + +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include +#include +namespace radar_object_tracker_utils +{ + +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time); + +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets); + +bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane); + +lanelet::ConstLanelets getClosestValidLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane, + const double max_angle_diff_from_lane); + +bool hasValidVelocityDirectionToLanelet( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity); + +} // namespace radar_object_tracker_utils + +#endif // RADAR_OBJECT_TRACKER__UTILS__RADAR_OBJECT_TRACKER_UTILS_HPP_ diff --git a/perception/radar_object_tracker/package.xml b/perception/radar_object_tracker/package.xml index 3697903e77e41..5be2ccef1e6cc 100644 --- a/perception/radar_object_tracker/package.xml +++ b/perception/radar_object_tracker/package.xml @@ -33,6 +33,7 @@ ament_lint_auto autoware_lint_common + gtest ament_cmake diff --git a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp index 460578699e780..dc87a180451d8 100644 --- a/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp +++ b/perception/radar_object_tracker/src/radar_object_tracker_node/radar_object_tracker_node.cpp @@ -14,6 +14,7 @@ #include "radar_object_tracker/radar_object_tracker_node/radar_object_tracker_node.hpp" +#include "radar_object_tracker/utils/radar_object_tracker_utils.hpp" #include "radar_object_tracker/utils/utils.hpp" #include @@ -36,158 +37,6 @@ #include #define EIGEN_MPL2_ONLY -namespace -{ -boost::optional getTransformAnonymous( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - // check if the frames are ready - std::string errstr; // This argument prevents error msg from being displayed in the terminal. - if (!tf_buffer.canTransform( - target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { - return boost::none; - } - - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - /*target*/ target_frame_id, /*src*/ source_frame_id, time, - rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what()); - return boost::none; - } -} - -// check if lanelet is close enough to the target lanelet -bool isDuplicated( - const std::pair & target_lanelet, - const lanelet::ConstLanelets & lanelets) -{ - const double CLOSE_LANELET_THRESHOLD = 0.1; - for (const auto & lanelet : lanelets) { - const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); - const auto lanelet_end_p = lanelet.centerline2d().back(); - const double dist = std::hypot( - target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); - if (dist < CLOSE_LANELET_THRESHOLD) { - return true; - } - } - - return false; -} - -// check if the lanelet is valid for object tracking -bool checkCloseLaneletCondition( - const std::pair & lanelet, const TrackedObject & object, - const double max_distance_from_lane, const double max_angle_diff_from_lane) -{ - // Step1. If we only have one point in the centerline, we will ignore the lanelet - if (lanelet.second.centerline().size() <= 1) { - return false; - } - - // Step2. Check if the obstacle is inside or close enough to the lanelet - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - if (!lanelet::geometry::inside(lanelet.second, search_point)) { - const auto distance = lanelet.first; - if (distance > max_distance_from_lane) { - return false; - } - } - - // Step3. Calculate the angle difference between the lane angle and obstacle angle - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet.second, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_yaw - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - const double abs_norm_delta = std::fabs(normalized_delta_yaw); - - // Step4. Check if the closest lanelet is valid, and add all - // of the lanelets that are below max_dist and max_delta_yaw - const double object_vel = object.kinematics.twist_with_covariance.twist.linear.x; - const bool is_yaw_reversed = M_PI - max_angle_diff_from_lane < abs_norm_delta && object_vel < 0.0; - if (is_yaw_reversed || abs_norm_delta < max_angle_diff_from_lane) { - return true; - } - - return false; -} - -lanelet::ConstLanelets getClosestValidLanelets( - const TrackedObject & object, const lanelet::LaneletMapPtr & lanelet_map_ptr, - const double max_distance_from_lane, const double max_angle_diff_from_lane) -{ - // obstacle point - lanelet::BasicPoint2d search_point( - object.kinematics.pose_with_covariance.pose.position.x, - object.kinematics.pose_with_covariance.pose.position.y); - - // nearest lanelet - std::vector> surrounding_lanelets = - lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); - - // No Closest Lanelets - if (surrounding_lanelets.empty()) { - return {}; - } - - lanelet::ConstLanelets closest_lanelets; - for (const auto & lanelet : surrounding_lanelets) { - // Check if the close lanelets meet the necessary condition for start lanelets and - // Check if similar lanelet is inside the closest lanelet - if ( - !checkCloseLaneletCondition( - lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || - isDuplicated(lanelet, closest_lanelets)) { - continue; - } - - closest_lanelets.push_back(lanelet.second); - } - - return closest_lanelets; -} - -bool hasValidVelocityDirectionToLanelet( - const TrackedObject & object, const lanelet::ConstLanelets & lanelets, - const double max_lateral_velocity) -{ - // get object velocity direction - const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); - const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; - const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; - const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); // local frame - const double object_vel_yaw_global = - tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); - const double object_vel = std::hypot(object_vel_x, object_vel_y); - - for (const auto & lanelet : lanelets) { - // get lanelet angle - const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet, object.kinematics.pose_with_covariance.pose.position); - const double delta_yaw = object_vel_yaw_global - lane_yaw; - const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); - - // get lateral velocity - const double lane_vel = object_vel * std::sin(normalized_delta_yaw); - // std::cout << "lane_vel: " << lane_vel << " , delta yaw:" << normalized_delta_yaw << " , - // object_vel " << object_vel <header.stamp); if (!self_transform) { return; @@ -387,8 +236,8 @@ std::shared_ptr RadarObjectTrackerNode::createNewTracker( void RadarObjectTrackerNode::onTimer() { rclcpp::Time current_time = this->now(); - const auto self_transform = - getTransformAnonymous(tf_buffer_, world_frame_id_, "base_link", current_time); + const auto self_transform = radar_object_tracker_utils::getTransformAnonymous( + tf_buffer_, world_frame_id_, "base_link", current_time); if (!self_transform) { return; } @@ -434,14 +283,15 @@ void RadarObjectTrackerNode::mapBasedNoiseFilter( for (auto itr = list_tracker.begin(); itr != list_tracker.end(); ++itr) { autoware_auto_perception_msgs::msg::TrackedObject object; (*itr)->getTrackedObject(time, object); - const auto closest_lanelets = getClosestValidLanelets( + const auto closest_lanelets = radar_object_tracker_utils::getClosestValidLanelets( object, lanelet_map_ptr_, max_distance_from_lane_, max_angle_diff_from_lane_); // 1. If the object is not close to any lanelet, delete the tracker const bool no_closest_lanelet = closest_lanelets.empty(); // 2. If the object velocity direction is not close to the lanelet direction, delete the tracker const bool is_velocity_direction_close_to_lanelet = - hasValidVelocityDirectionToLanelet(object, closest_lanelets, max_lateral_velocity_); + radar_object_tracker_utils::hasValidVelocityDirectionToLanelet( + object, closest_lanelets, max_lateral_velocity_); if (no_closest_lanelet || !is_velocity_direction_close_to_lanelet) { // std::cout << "object removed due to map based noise filter" << " no close lanelet: " << // no_closest_lanelet << " velocity direction flag:" << is_velocity_direction_close_to_lanelet diff --git a/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp new file mode 100644 index 0000000000000..870e77cc20d41 --- /dev/null +++ b/perception/radar_object_tracker/src/utils/radar_object_tracker_utils.cpp @@ -0,0 +1,154 @@ +// Copyright 2024 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_object_tracker/utils/radar_object_tracker_utils.hpp" + +namespace radar_object_tracker_utils +{ + +boost::optional getTransformAnonymous( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + std::string errstr; + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("radar_object_tracker"), ex.what()); + return boost::none; + } +} + +bool isDuplicated( + const std::pair & target_lanelet, + const lanelet::ConstLanelets & lanelets) +{ + const double CLOSE_LANELET_THRESHOLD = 0.1; + for (const auto & lanelet : lanelets) { + const auto target_lanelet_end_p = target_lanelet.second.centerline2d().back(); + const auto lanelet_end_p = lanelet.centerline2d().back(); + const double dist = std::hypot( + target_lanelet_end_p.x() - lanelet_end_p.x(), target_lanelet_end_p.y() - lanelet_end_p.y()); + if (dist < CLOSE_LANELET_THRESHOLD) { + return true; + } + } + + return false; +} + +bool checkCloseLaneletCondition( + const std::pair & lanelet, + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const double max_distance_from_lane, const double max_angle_diff_from_lane) +{ + if (lanelet.second.centerline().size() <= 1) { + return false; + } + + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + if (!lanelet::geometry::inside(lanelet.second, search_point)) { + const auto distance = lanelet.first; + if (distance > max_distance_from_lane) { + return false; + } + } + + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet.second, object.kinematics.pose_with_covariance.pose.position); + double object_motion_yaw = object_yaw; + bool velocity_is_reverted = object.kinematics.twist_with_covariance.twist.linear.x < 0.0; + if (velocity_is_reverted) { + object_motion_yaw = tier4_autoware_utils::normalizeRadian(object_yaw + M_PI); + } + const double delta_yaw = object_motion_yaw - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw); + + if (abs_norm_delta_yaw > max_angle_diff_from_lane) { + return false; + } + + return true; +} + +lanelet::ConstLanelets getClosestValidLanelets( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::LaneletMapPtr & lanelet_map_ptr, const double max_distance_from_lane, + const double max_angle_diff_from_lane) +{ + lanelet::BasicPoint2d search_point( + object.kinematics.pose_with_covariance.pose.position.x, + object.kinematics.pose_with_covariance.pose.position.y); + + std::vector> surrounding_lanelets = + lanelet::geometry::findNearest(lanelet_map_ptr->laneletLayer, search_point, 10); + + if (surrounding_lanelets.empty()) { + return {}; + } + + lanelet::ConstLanelets closest_lanelets; + for (const auto & lanelet : surrounding_lanelets) { + if ( + !checkCloseLaneletCondition( + lanelet, object, max_distance_from_lane, max_angle_diff_from_lane) || + isDuplicated(lanelet, closest_lanelets)) { + continue; + } + + closest_lanelets.push_back(lanelet.second); + } + + return closest_lanelets; +} + +bool hasValidVelocityDirectionToLanelet( + const autoware_auto_perception_msgs::msg::TrackedObject & object, + const lanelet::ConstLanelets & lanelets, const double max_lateral_velocity) +{ + const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); + const double object_vel_x = object.kinematics.twist_with_covariance.twist.linear.x; + const double object_vel_y = object.kinematics.twist_with_covariance.twist.linear.y; + const double object_vel_yaw = std::atan2(object_vel_y, object_vel_x); + const double object_vel_yaw_global = + tier4_autoware_utils::normalizeRadian(object_yaw + object_vel_yaw); + const double object_vel = std::hypot(object_vel_x, object_vel_y); + + for (const auto & lanelet : lanelets) { + const double lane_yaw = lanelet::utils::getLaneletAngle( + lanelet, object.kinematics.pose_with_covariance.pose.position); + const double delta_yaw = object_vel_yaw_global - lane_yaw; + const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw); + + const double lane_vel = object_vel * std::sin(normalized_delta_yaw); + if (std::fabs(lane_vel) < max_lateral_velocity) { + return true; + } + } + return false; +} + +} // namespace radar_object_tracker_utils diff --git a/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp new file mode 100644 index 0000000000000..50b1de7132f6f --- /dev/null +++ b/perception/radar_object_tracker/test/test_radar_object_tracker_utils.cpp @@ -0,0 +1,121 @@ +// Copyright 2024 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_object_tracker/utils/radar_object_tracker_utils.hpp" + +#include + +using autoware_auto_perception_msgs::msg::TrackedObject; +using radar_object_tracker_utils::checkCloseLaneletCondition; + +// helper function to create a dummy straight lanelet +lanelet::Lanelet createDummyStraightLanelet(double length, double width) +{ + lanelet::LineString3d left_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, 0, 0), + lanelet::Point3d(lanelet::utils::getId(), length, 0, 0)}); + lanelet::LineString3d right_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, width, 0), + lanelet::Point3d(lanelet::utils::getId(), length, width, 0)}); + lanelet::LineString3d center_line( + lanelet::utils::getId(), {lanelet::Point3d(lanelet::utils::getId(), 0, width / 2, 0), + lanelet::Point3d(lanelet::utils::getId(), length, width / 2, 0)}); + + // Correct constructor call + return lanelet::Lanelet( + lanelet::utils::getId(), left_line, right_line, lanelet::AttributeMap(), + lanelet::RegulatoryElementPtrs()); +} + +// helper function to create a dummy tracked object +TrackedObject createDummyTrackedObject(double x, double y, double yaw, double velocity) +{ + TrackedObject obj; + obj.kinematics.pose_with_covariance.pose.position.x = x; + obj.kinematics.pose_with_covariance.pose.position.y = y; + tf2::Quaternion q; + q.setRPY(0, 0, yaw); + obj.kinematics.pose_with_covariance.pose.orientation = tf2::toMsg(q); + obj.kinematics.twist_with_covariance.twist.linear.x = velocity; + return obj; +} + +// 1. Test checkCloseLaneletCondition +// 1. Inside lanelet +TEST(CheckCloseLaneletConditionTest, InsideLanelet) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(50.0, 1.5, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({0.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_TRUE(result); +} + +// 2. Outside lanelet but close +TEST(CheckCloseLaneletConditionTest, OutsideLaneletButClose) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(5.0, 50.0, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({3.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_TRUE(result); +} + +// 3. Outside lanelet and far +TEST(CheckCloseLaneletConditionTest, OutsideLaneletAndFar) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + TrackedObject obj = createDummyTrackedObject(10.0, 50.0, 0.0, 10.0); + + bool result = checkCloseLaneletCondition({10.0, lanelet}, obj, 5.0, M_PI / 4); + EXPECT_FALSE(result); +} + +// 4. Inside but has multiple angle difference and velocity conditions +TEST(CheckCloseLaneletConditionTest, AngleDifferenceTooLarge) +{ + auto lanelet = createDummyStraightLanelet(100.0, 3.0); + constexpr double eps = 1e-6; + // 1. forward velocity and angle difference is 0 + const TrackedObject obj1_1 = createDummyTrackedObject(50.0, 1.5, 0.0, 10.0); + const TrackedObject obj1_2 = createDummyTrackedObject(50.0, 1.5, eps, 10.0); + const TrackedObject obj1_3 = createDummyTrackedObject(50.0, 1.5, -eps, 10.0); + // 2. forward velocity and angle difference is Inverse + const TrackedObject obj2_1 = createDummyTrackedObject(50.0, 1.5, M_PI, 10.0); + const TrackedObject obj2_2 = createDummyTrackedObject(50.0, 1.5, M_PI + eps, 10.0); + const TrackedObject obj2_3 = createDummyTrackedObject(50.0, 1.5, M_PI - eps, 10.0); + // 3. backward velocity and angle difference is 0 + const TrackedObject obj3_1 = createDummyTrackedObject(50.0, 1.5, M_PI, -10.0); + const TrackedObject obj3_2 = createDummyTrackedObject(50.0, 1.5, M_PI + eps, -10.0); + const TrackedObject obj3_3 = createDummyTrackedObject(50.0, 1.5, M_PI - eps, -10.0); + // 4. backward velocity and angle difference is Inverse + const TrackedObject obj4_1 = createDummyTrackedObject(50.0, 1.5, 0.0, -10.0); + const TrackedObject obj4_2 = createDummyTrackedObject(50.0, 1.5, eps, -10.0); + const TrackedObject obj4_3 = createDummyTrackedObject(50.0, 1.5, -eps, -10.0); + + // 1 and 3 should be true, 2 and 4 should be false + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_1, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_2, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj1_3, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_1, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_2, 5.0, M_PI / 4)); + EXPECT_TRUE(checkCloseLaneletCondition({0.0, lanelet}, obj3_3, 5.0, M_PI / 4)); + + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_1, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_2, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj2_3, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_1, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_2, 5.0, M_PI / 4)); + EXPECT_FALSE(checkCloseLaneletCondition({0.0, lanelet}, obj4_3, 5.0, M_PI / 4)); +} diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index 9d7e8b4d4729d..04a96391ca165 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -61,6 +61,22 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option published_time_publisher_ = std::make_unique(this); } +static autoware_auto_perception_msgs::msg::ObjectClassification::_label_type get_label( + const autoware_auto_perception_msgs::msg::DetectedObject::_classification_type & classification) +{ + if (classification.empty()) { + return Label::UNKNOWN; + } + return classification.front().label; +} + +static bool label_is_vehicle( + const autoware_auto_perception_msgs::msg::ObjectClassification::_label_type & label) +{ + return Label::CAR == label || Label::TRUCK == label || Label::BUS == label || + Label::TRAILER == label; +} + void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstSharedPtr input_msg) { stop_watch_ptr_->toc("processing_time", true); @@ -76,11 +92,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared // Estimate shape for each object and pack msg for (const auto & feature_object : input_msg->feature_objects) { const auto & object = feature_object.object; - const auto & label = object.classification.front().label; + const auto label = get_label(object.classification); + const auto is_vehicle = label_is_vehicle(label); const auto & feature = feature_object.feature; - const bool is_vehicle = Label::CAR == label || Label::TRUCK == label || Label::BUS == label || - Label::TRAILER == label; - // convert ros to pcl pcl::PointCloud::Ptr cluster(new pcl::PointCloud); pcl::fromROSMsg(feature.cluster, *cluster); diff --git a/perception/tracking_object_merger/src/data_association/data_association.cpp b/perception/tracking_object_merger/src/data_association/data_association.cpp index e7ab6077250f8..7cd7d532d07f5 100644 --- a/perception/tracking_object_merger/src/data_association/data_association.cpp +++ b/perception/tracking_object_merger/src/data_association/data_association.cpp @@ -180,13 +180,6 @@ double DataAssociation::calcScoreBetweenObjects( const std::uint8_t object0_label = object_recognition_utils::getHighestProbLabel(object0.classification); - std::vector tracker_pose = { - object1.kinematics.pose_with_covariance.pose.position.x, - object1.kinematics.pose_with_covariance.pose.position.y}; - std::vector measurement_pose = { - object0.kinematics.pose_with_covariance.pose.position.x, - object0.kinematics.pose_with_covariance.pose.position.y}; - double score = 0.0; if (can_assign_matrix_(object1_label, object0_label)) { const double max_dist = max_dist_matrix_(object1_label, object0_label); diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp new file mode 100644 index 0000000000000..63474c5e458cd --- /dev/null +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -0,0 +1,124 @@ +// Copyright 2024 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 AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ +#define AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ +#include +#include + +#include +#include +#include + +#include +#include + +namespace autoware_planning_test_manager::utils +{ +using autoware_planning_msgs::msg::LaneletRoute; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using route_handler::RouteHandler; +using RouteSections = std::vector; + +Pose createPoseFromLaneID(const lanelet::Id & lane_id) +{ + auto map_bin_msg = test_utils::makeMapBinMsg(); + // create route_handler + auto route_handler = std::make_shared(); + route_handler->setMap(map_bin_msg); + + // get middle idx of the lanelet + const auto lanelet = route_handler->getLaneletsFromId(lane_id); + const auto center_line = lanelet.centerline(); + const size_t middle_point_idx = std::floor(center_line.size() / 2.0); + + // get middle position of the lanelet + geometry_msgs::msg::Point middle_pos; + middle_pos.x = center_line[middle_point_idx].x(); + middle_pos.y = center_line[middle_point_idx].y(); + + // get next middle position of the lanelet + geometry_msgs::msg::Point next_middle_pos; + next_middle_pos.x = center_line[middle_point_idx + 1].x(); + next_middle_pos.y = center_line[middle_point_idx + 1].y(); + + // calculate middle pose + geometry_msgs::msg::Pose middle_pose; + middle_pose.position = middle_pos; + const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); + middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); + + return middle_pose; +} + +// Function to create a route from given start and goal lanelet ids +// start pose and goal pose are set to the middle of the lanelet +LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) +{ + LaneletRoute route; + route.header.frame_id = "map"; + auto start_pose = createPoseFromLaneID(start_lane_id); + auto goal_pose = createPoseFromLaneID(goal_lane_id); + route.start_pose = start_pose; + route.goal_pose = goal_pose; + + auto map_bin_msg = test_utils::makeMapBinMsg(); + // create route_handler + auto route_handler = std::make_shared(); + route_handler->setMap(map_bin_msg); + + LaneletRoute route_msg; + RouteSections route_sections; + lanelet::ConstLanelets all_route_lanelets; + + // Plan the path between checkpoints (start and goal poses) + lanelet::ConstLanelets path_lanelets; + if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) { + return route_msg; + } + + // Add all path_lanelets to all_route_lanelets + for (const auto & lane : path_lanelets) { + all_route_lanelets.push_back(lane); + } + // create local route sections + route_handler->setRouteLanelets(path_lanelets); + const auto local_route_sections = route_handler->createMapSegments(path_lanelets); + route_sections = + test_utils::combineConsecutiveRouteSections(route_sections, local_route_sections); + for (const auto & route_section : route_sections) { + for (const auto & primitive : route_section.primitives) { + std::cerr << "primitive: " << primitive.id << std::endl; + } + std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl; + } + route_handler->setRouteLanelets(all_route_lanelets); + route.segments = route_sections; + + route.allow_modification = false; + return route; +} + +Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) +{ + Odometry current_odometry; + current_odometry.pose.pose = createPoseFromLaneID(lane_id); + current_odometry.header.frame_id = "map"; + + return current_odometry; +} + +} // namespace autoware_planning_test_manager::utils +#endif // AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_ diff --git a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp index 3aee408a76306..9ee162ec02d1f 100644 --- a/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp +++ b/planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp @@ -15,7 +15,9 @@ #include "motion_utils/trajectory/conversion.hpp" #include +#include #include +#include namespace planning_test_utils { @@ -115,7 +117,7 @@ void PlanningInterfaceTestManager::publishInitialPose( if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, initial_pose_pub_, - test_utils::makeInitialPoseFromLaneId(10291)); + autoware_planning_test_manager::utils::makeInitialPoseFromLaneId(10291)); } else { test_utils::publishToTargetNode( test_node_, target_node, topic_name, initial_pose_pub_, test_utils::makeInitialPose(shift)); @@ -256,7 +258,7 @@ void PlanningInterfaceTestManager::publishBehaviorNominalRoute( if (module_name == ModuleName::START_PLANNER) { test_utils::publishToTargetNode( test_node_, target_node, topic_name, behavior_normal_route_pub_, - test_utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); + autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId(10291, 10333), 5); } else { test_utils::publishToTargetNode( test_node_, target_node, topic_name, behavior_normal_route_pub_, diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 2a2fbe4340fa8..8e016e2b63391 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -10,7 +10,7 @@ - + diff --git a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz index 002af580c2f00..62b4c9b75ec87 100644 --- a/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz +++ b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz @@ -130,7 +130,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /autoware_static_centerline_generator/input_centerline + Value: /static_centerline_generator/input_centerline Value: true View Drivable Area: Alpha: 0.9990000128746033 @@ -179,7 +179,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /autoware_static_centerline_generator/output_centerline + Value: /static_centerline_generator/output_centerline Value: true View Footprint: Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /autoware_static_centerline_generator/debug/raw_centerline + Value: /static_centerline_generator/debug/raw_centerline Value: false View Drivable Area: Alpha: 0.9990000128746033 @@ -268,7 +268,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /autoware_static_centerline_generator/debug/unsafe_footprints + Value: /static_centerline_generator/debug/unsafe_footprints Value: true Enabled: false Name: debug diff --git a/planning/autoware_static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py index f37737a0de4e4..08bff8b80dcb7 100755 --- a/planning/autoware_static_centerline_generator/scripts/app.py +++ b/planning/autoware_static_centerline_generator/scripts/app.py @@ -51,7 +51,7 @@ def get_map(): map_id = map_uuid # create client - cli = create_client(LoadMap, "/planning/autoware_static_centerline_generator/load_map") + cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") # request map loading req = LoadMap.Request(map=data["map"]) @@ -85,7 +85,7 @@ def post_planned_route(): print("map_id is not correct.") # create client - cli = create_client(PlanRoute, "/planning/autoware_static_centerline_generator/plan_route") + cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") # request route planning req = PlanRoute.Request( @@ -123,7 +123,7 @@ def post_planned_path(): print("map_id is not correct.") # create client - cli = create_client(PlanPath, "/planning/autoware_static_centerline_generator/plan_path") + cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") # request path planning route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] diff --git a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py index cec4e5b457c7f..fec3d21d20bec 100755 --- a/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py @@ -155,7 +155,7 @@ def __init__(self): transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) self.sub_whole_centerline = self.create_subscription( Trajectory, - "/autoware_static_centerline_generator/output_whole_centerline", + "/static_centerline_generator/output_whole_centerline", self.onWholeCenterline, transient_local_qos, ) diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 8388bb61748f1..74002916bb23c 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -21,6 +21,7 @@ #include "lanelet2_extension/utility/utilities.hpp" #include "map_loader/lanelet2_map_loader_node.hpp" #include "map_projection_loader/load_info_from_lanelet2_map.hpp" +#include "map_projection_loader/map_projection_loader.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -36,7 +37,6 @@ #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" -#include #include #include @@ -169,7 +169,7 @@ std::vector resample_trajectory_points( StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const rclcpp::NodeOptions & node_options) -: Node("autoware_static_centerline_generator", node_options) +: Node("static_centerline_generator", node_options) { // publishers pub_map_bin_ = @@ -204,8 +204,10 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const auto selected_centerline = std::vector( c.centerline.begin() + traj_range_indices_.first, c.centerline.begin() + traj_range_indices_.second + 1); + const auto selected_route_lane_ids = get_route_lane_ids_from_points(selected_centerline); save_map( - lanelet2_output_file_path, CenterlineWithRoute{selected_centerline, c.route_lane_ids}); + lanelet2_output_file_path, + CenterlineWithRoute{selected_centerline, selected_route_lane_ids}); } }); sub_traj_resample_interval_ = create_subscription( @@ -217,19 +219,19 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_load_map_ = create_service( - "/planning/autoware_static_centerline_generator/load_map", + "/planning/static_centerline_generator/load_map", std::bind( &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_route_ = create_service( - "/planning/autoware_static_centerline_generator/plan_route", + "/planning/static_centerline_generator/plan_route", std::bind( &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_path_ = create_service( - "/planning/autoware_static_centerline_generator/plan_path", + "/planning/static_centerline_generator/plan_path", std::bind( &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, std::placeholders::_2), @@ -280,6 +282,8 @@ void StaticCenterlineGeneratorNode::run() load_map(lanelet2_input_file_path); const auto centerline_with_route = generate_centerline_with_route(); traj_range_indices_ = std::make_pair(0, centerline_with_route.centerline.size() - 1); + + evaluate(centerline_with_route.route_lane_ids, centerline_with_route.centerline); save_map(lanelet2_output_file_path, centerline_with_route); centerline_with_route_ = centerline_with_route; @@ -304,19 +308,7 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); - const auto start_lanelets = - route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.front().pose); - const auto end_lanelets = - route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.back().pose); - if (start_lanelets.empty() || end_lanelets.empty()) { - RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); - return CenterlineWithRoute{}; - } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - + const auto route_lane_ids = get_route_lane_ids_from_points(bag_centerline); return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( @@ -337,6 +329,24 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout return centerline_with_route; } +std::vector StaticCenterlineGeneratorNode::get_route_lane_ids_from_points( + const std::vector & points) +{ + const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.front().pose); + const auto end_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(points.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's points are not found."); + return std::vector{}; + } + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + if (start_lanelet_id == end_lanelet_id) { + return std::vector{start_lanelet_id}; + } + return plan_route(start_lanelet_id, end_lanelet_id); +} + void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // copy the input LL2 map to the temporary file for debugging @@ -349,21 +359,18 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { // load map - const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); + map_projector_info_ = + std::make_unique(load_info_from_lanelet2_map(lanelet2_input_file_path)); const auto map_ptr = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_); if (!map_ptr) { return nullptr; } - // NOTE: generate map projector for lanelet::write(). - // Without this, lat/lon of the generated LL2 map will be wrong. - map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); - - // NOTE: The original map is stored here since the various ids in the lanelet map will change - // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. + // NOTE: The original map is stored here since the centerline will be added to all the + // lanelet when lanelet::utils::overwriteLaneletCenterline is called. original_map_ptr_ = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, *map_projector_info_); // overwrite more dense centerline lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); @@ -639,11 +646,13 @@ void StaticCenterlineGeneratorNode::save_map( const auto route_lanelets = utils::get_lanelets_from_ids(*route_handler_ptr_, c.route_lane_ids); // update centerline in map - utils::update_centerline(*route_handler_ptr_, route_lanelets, c.centerline); + utils::update_centerline(original_map_ptr_, route_lanelets, c.centerline); RCLCPP_INFO(get_logger(), "Updated centerline in map."); // save map with modified center line - lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_); + std::filesystem::create_directory("/tmp/static_centerline_generator"); // TODO(murooka) + const auto map_projector = geography_utils::get_lanelet2_projector(*map_projector_info_); + lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector); RCLCPP_INFO(get_logger(), "Saved map."); // copy the output LL2 map to the temporary file for debugging diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp index 9baa1c3fbb892..dfe5af68c270b 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp @@ -23,11 +23,10 @@ #include "type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include - #include "std_msgs/msg/bool.hpp" #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int32.hpp" +#include "tier4_map_msgs/msg/map_projector_info.hpp" #include #include @@ -39,6 +38,7 @@ namespace autoware::static_centerline_generator using autoware_static_centerline_generator::srv::LoadMap; using autoware_static_centerline_generator::srv::PlanPath; using autoware_static_centerline_generator::srv::PlanRoute; +using tier4_map_msgs::msg::MapProjectorInfo; struct CenterlineWithRoute { @@ -66,6 +66,8 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // plan centerline CenterlineWithRoute generate_centerline_with_route(); + std::vector get_route_lane_ids_from_points( + const std::vector & points); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -80,7 +82,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node lanelet::LaneletMapPtr original_map_ptr_{nullptr}; HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; - std::unique_ptr map_projector_{nullptr}; + std::unique_ptr map_projector_info_{nullptr}; std::pair traj_range_indices_{0, 0}; std::optional centerline_with_route_{std::nullopt}; diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 4ea56e10935b2..2dbd82772f7ef 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -35,12 +35,19 @@ nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs: return odometry_ptr; } -lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0) +lanelet::Point3d createPoint3d(const double x, const double y, const double z) { lanelet::Point3d point(lanelet::utils::getId()); + + // position + point.x() = x; + point.y() = y; + point.z() = z; + + // attributes point.setAttribute("local_x", x); point.setAttribute("local_y", y); - point.setAttribute("ele", z); + // NOTE: It seems that the attribute "ele" is assigned automatically. return point; } @@ -76,11 +83,13 @@ geometry_msgs::msg::Pose get_center_pose( geometry_msgs::msg::Point middle_pos; middle_pos.x = center_line[middle_point_idx].x(); middle_pos.y = center_line[middle_point_idx].y(); + middle_pos.z = center_line[middle_point_idx].z(); // get next middle position of the lanelet geometry_msgs::msg::Point next_middle_pos; next_middle_pos.x = center_line[middle_point_idx + 1].x(); next_middle_pos.y = center_line[middle_point_idx + 1].y(); + next_middle_pos.z = center_line[middle_point_idx + 1].z(); // calculate middle pose geometry_msgs::msg::Pose middle_pose; @@ -119,13 +128,13 @@ PathWithLaneId get_path_with_lane_id( } void update_centerline( - RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline) { // get lanelet as reference to update centerline lanelet::Lanelets lanelets_ref; for (const auto & lanelet : lanelets) { - for (auto & lanelet_ref : route_handler.getLaneletMapPtr()->laneletLayer) { + for (auto & lanelet_ref : lanelet_map_ptr->laneletLayer) { if (lanelet_ref.id() == lanelet.id()) { lanelets_ref.push_back(lanelet_ref); } @@ -148,13 +157,13 @@ void update_centerline( // set center point centerline.push_back(center_point); - route_handler.getLaneletMapPtr()->add(center_point); + lanelet_map_ptr->add(center_point); break; } if (!centerline.empty()) { // set centerline - route_handler.getLaneletMapPtr()->add(centerline); + lanelet_map_ptr->add(centerline); lanelet_ref.setCenterline(centerline); // prepare new centerline @@ -166,7 +175,7 @@ void update_centerline( auto & lanelet_ref = lanelets_ref.at(lanelet_idx); // set centerline - route_handler.getLaneletMapPtr()->add(centerline); + lanelet_map_ptr->add(centerline); lanelet_ref.setCenterline(centerline); } } diff --git a/planning/autoware_static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp index 6c6257bee59a3..1d7eb1f18cc44 100644 --- a/planning/autoware_static_centerline_generator/src/utils.hpp +++ b/planning/autoware_static_centerline_generator/src/utils.hpp @@ -42,7 +42,7 @@ PathWithLaneId get_path_with_lane_id( const double nearest_ego_yaw_threshold); void update_centerline( - RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, + lanelet::LaneletMapPtr lanelet_map_ptr, const lanelet::ConstLanelets & lanelets, const std::vector & new_centerline); MarkerArray create_footprint_marker( diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp index 7ae5a97b77fff..1e83769dde599 100644 --- a/planning/behavior_path_avoidance_module/src/utils.cpp +++ b/planning/behavior_path_avoidance_module/src/utils.cpp @@ -912,8 +912,14 @@ double getRoadShoulderDistance( } } + const auto envelope_polygon_width = + boost::geometry::area(object.envelope_poly) / + std::max(object.length, 1e-3); // prevent division by zero + { - const auto p2 = calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -1.0 : 1.0), 0.0).position; + const auto p2 = + calcOffsetPose(p_tmp, 0.0, (isOnRight(object) ? -0.5 : 0.5) * envelope_polygon_width, 0.0) + .position; const auto opt_intersect = tier4_autoware_utils::intersect(p1.second, p2, bound.at(i - 1), bound.at(i)); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 1052a283efc47..2d88a820e0fae 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -1974,11 +1974,11 @@ bool NormalLaneChange::calcAbortPath() auto reference_lane_segment = prev_module_output_.path; { - const auto terminal_path = - calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); - if (terminal_path) { - reference_lane_segment = terminal_path->path; - } + // const auto terminal_path = + // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // if (terminal_path) { + // reference_lane_segment = terminal_path->path; + // } const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 03f8c8d2a5696..beaa1ae6c5263 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -32,8 +32,8 @@ Behavior Path Planner has following scene modules | Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](../behavior_path_avoidance_by_lane_change_module/README.md) | | Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](../behavior_path_lane_change_module/README.md) | | External Lane Change | WIP | LINK | -| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | -| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](../behavior_path_goal_planner_module/README.md) | +| Start Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](../behavior_path_start_planner_module/README.md) | | Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](../behavior_path_side_shift_module/README.md) | !!! Note diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index b4e1ffce8104a..bd7949a83a1d7 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -187,7 +187,6 @@ class SceneModuleInterface RCLCPP_DEBUG(getLogger(), "%s %s", name_.c_str(), __func__); clearWaitingApproval(); - removeRTCStatus(); unlockNewModuleLaunch(); unlockOutputPath(); steering_factor_interface_ptr_->clearSteeringFactors(); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 0bea7c08c6601..2eb56104ec831 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -103,6 +103,7 @@ class SceneModuleManagerInterface { for (const auto & [module_name, ptr] : rtc_interface_ptr_map_) { if (ptr) { + ptr->removeExpiredCooperateStatus(); ptr->publishCooperateStatus(rclcpp::Clock(RCL_ROS_TIME).now()); } } diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index aa9613a35dd7c..48f263e14ba72 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -352,6 +352,7 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { + // if (1)the observation is not UNKNOWN or (2)the very first observation is UNKNOWN planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = traffic_signal; } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 27db42d36f08b..3a2b474e3d730 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -247,7 +247,11 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface void removeRTCStatus(const UUID & uuid) { rtc_interface_.removeCooperateStatus(uuid); } - void publishRTCStatus(const Time & stamp) { rtc_interface_.publishCooperateStatus(stamp); } + void publishRTCStatus(const Time & stamp) + { + rtc_interface_.removeExpiredCooperateStatus(); + rtc_interface_.publishCooperateStatus(stamp); + } UUID getUUID(const int64_t & module_id) const; diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp index 803682489ebde..eb43e45d55711 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -272,7 +272,10 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( for (const auto & scene_module : copied_scene_modules) { if (isModuleExpired(scene_module)) { - removeRTCStatus(getUUID(scene_module->getModuleId())); + const UUID uuid = getUUID(scene_module->getModuleId()); + updateRTCStatus( + uuid, scene_module->isSafe(), State::SUCCEEDED, std::numeric_limits::lowest(), + clock_->now()); removeUUID(scene_module->getModuleId()); unregisterModule(scene_module); } diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index 33bb471e5920c..544e14f130a7e 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -8,6 +8,7 @@ Satoshi Ota Tomoya Kimura Shumpei Wakabayashi + Mamoru Sobue Apache License 2.0 diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 6e6871e20a354..edddef5cef4d8 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -62,7 +62,7 @@ void TrafficLightModuleManager::modifyPathVelocity( stop_reason_array.header.stamp = path->header.stamp; first_stop_path_point_index_ = static_cast(path->points.size() - 1); - first_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); + nearest_ref_stop_path_point_index_ = static_cast(path->points.size() - 1); for (const auto & scene_module : scene_modules_) { tier4_planning_msgs::msg::StopReason stop_reason; std::shared_ptr traffic_light_scene_module( @@ -85,8 +85,8 @@ void TrafficLightModuleManager::modifyPathVelocity( } if ( traffic_light_scene_module->getFirstRefStopPathPointIndex() < - first_ref_stop_path_point_index_) { - first_ref_stop_path_point_index_ = + nearest_ref_stop_path_point_index_) { + nearest_ref_stop_path_point_index_ = traffic_light_scene_module->getFirstRefStopPathPointIndex(); if ( traffic_light_scene_module->getTrafficLightModuleState() != @@ -126,15 +126,14 @@ void TrafficLightModuleManager::launchNewModules( // Use lanelet_id to unregister module when the route is changed const auto lane_id = traffic_light_reg_elem.second.id(); - const auto module_id = lane_id; - if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) { + if (!isModuleRegisteredFromExistingAssociatedModule(lane_id)) { registerModule(std::make_shared( - module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, - planner_param_, logger_.get_child("traffic_light_module"), clock_)); - generateUUID(module_id); + lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second, planner_param_, + logger_.get_child("traffic_light_module"), clock_)); + generateUUID(lane_id); updateRTCStatus( - getUUID(module_id), true, State::WAITING_FOR_EXECUTION, - std::numeric_limits::lowest(), path.header.stamp); + getUUID(lane_id), true, State::WAITING_FOR_EXECUTION, std::numeric_limits::lowest(), + path.header.stamp); } } } @@ -146,9 +145,10 @@ TrafficLightModuleManager::getModuleExpiredFunction( const auto lanelet_id_set = planning_utils::getLaneletIdSetOnPath( path, planner_data_->route_handler_->getLaneletMapPtr(), planner_data_->current_odometry->pose); - return [this, lanelet_id_set](const std::shared_ptr & scene_module) { + return [this, lanelet_id_set]( + [[maybe_unused]] const std::shared_ptr & scene_module) { for (const auto & id : lanelet_id_set) { - if (isModuleRegisteredFromRegElement(id, scene_module->getModuleId())) { + if (isModuleRegisteredFromExistingAssociatedModule(id)) { return false; } } @@ -156,23 +156,6 @@ TrafficLightModuleManager::getModuleExpiredFunction( }; } -bool TrafficLightModuleManager::isModuleRegisteredFromRegElement( - const lanelet::Id & id, const size_t module_id) const -{ - const auto lane = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(id); - - const auto registered_lane = - planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(module_id); - for (const auto & registered_element : registered_lane.regulatoryElementsAs()) { - for (const auto & element : lane.regulatoryElementsAs()) { - if (hasSameTrafficLight(element, registered_element)) { - return true; - } - } - } - return false; -} - bool TrafficLightModuleManager::isModuleRegisteredFromExistingAssociatedModule( const lanelet::Id & id) const { diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 87213d8a5ed3f..959ef2f91d36c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -58,7 +58,7 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC // Debug rclcpp::Publisher::SharedPtr pub_tl_state_; - std::optional first_ref_stop_path_point_index_; + std::optional nearest_ref_stop_path_point_index_; }; class TrafficLightModulePlugin : public PluginWrapper diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index 413ce78beacee..0158251bb42b4 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -162,11 +162,10 @@ bool calcStopPointAndInsertIndex( } // namespace TrafficLightModule::TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) -: SceneModuleInterface(module_id, logger, clock), +: SceneModuleInterface(lane_id, logger, clock), lane_id_(lane_id), traffic_light_reg_elem_(traffic_light_reg_elem), lane_(lane), @@ -365,7 +364,7 @@ bool TrafficLightModule::findValidTrafficSignal(TrafficSignalStamped & valid_tra { // get traffic signal associated with the regulatory element id const auto traffic_signal_stamped_opt = planner_data_->getTrafficSignal( - traffic_light_reg_elem_.id(), true /* traffic light module keeps last observation */); + traffic_light_reg_elem_.id(), false /* traffic light module does not keep last observation */); if (!traffic_signal_stamped_opt) { RCLCPP_WARN_STREAM_ONCE( logger_, "the traffic signal data associated with regulatory element id " diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 102ddbe2e9fa8..d6a409ca7034a 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -66,9 +66,8 @@ class TrafficLightModule : public SceneModuleInterface public: TrafficLightModule( - const int64_t module_id, const int64_t lane_id, - const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane, - const PlannerParam & planner_param, const rclcpp::Logger logger, + const int64_t lane_id, const lanelet::TrafficLight & traffic_light_reg_elem, + lanelet::ConstLanelet lane, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp index 9f1a9d206e376..0c2cb98ffbb35 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -48,6 +48,8 @@ class DefaultPlanner : public mission_planner::PlannerPlugin void initialize(rclcpp::Node * node, const HADMapBin::ConstSharedPtr msg) override; bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; + void updateRoute(const PlannerPlugin::LaneletRoute & route) override; + void clearRoute() override; MarkerArray visualize(const LaneletRoute & route) const override; MarkerArray visualize_debug_footprint(tier4_autoware_utils::LinearRing2d goal_footprint_) const; vehicle_info_util::VehicleInfo vehicle_info_; @@ -102,9 +104,6 @@ class DefaultPlanner : public mission_planner::PlannerPlugin * route_sections) and return the z-aligned goal position */ Pose refine_goal_height(const Pose & goal, const RouteSections & route_sections); - - void updateRoute(const PlannerPlugin::LaneletRoute & route); - void clearRoute(); }; } // namespace mission_planner::lanelet2 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index c368265ea0f66..3f18c6e1728c6 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -55,15 +55,19 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const std::vector & traj_points, const vehicle_info_util::VehicleInfo & vehicle_info, const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; - std::vector convertToObstacles(const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points) const; std::tuple, std::vector, std::vector> determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles); std::vector decimateTrajectoryPoints( - const std::vector & traj_points) const; + const Odometry & odometry, const std::vector & traj_points) const; std::optional createStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lateral_dist) const; + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lateral_dist) const; bool isStopObstacle(const uint8_t label) const; bool isInsideCruiseObstacle(const uint8_t label) const; bool isOutsideCruiseObstacle(const uint8_t label) const; @@ -88,12 +92,14 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool isObstacleCrossing( const std::vector & traj_points, const Obstacle & obstacle) const; double calcCollisionTimeMargin( - const std::vector & collision_points, + const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const; std::optional createSlowDownObstacle( - const std::vector & traj_points, const Obstacle & obstacle, - const double precise_lat_dist); - PlannerData createPlannerData(const std::vector & traj_points) const; + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist); + PlannerData createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, + const std::vector & traj_points) const; void checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 1d490abca291a..71d0f84637abc 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -47,8 +47,7 @@ std::vector getCollisionPoints( const std::vector & traj_points, const std::vector & traj_polygons, const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, const rclcpp::Time & current_time, const bool is_driving_forward, - std::vector & collision_index, - const double max_lat_dist = std::numeric_limits::max(), + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); } // namespace polygon_utils diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 43400acb86bd9..f637911dae903 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -484,12 +484,17 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - if ( - !ego_odom_sub_.updateLatestData() || !objects_sub_.updateLatestData() || - !acc_sub_.updateLatestData()) { + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + const auto objects_ptr = objects_sub_.takeData(); + const auto acc_ptr = acc_sub_.takeData(); + if (!ego_odom_ptr || !objects_ptr || !acc_ptr) { return; } + const auto & ego_odom = *ego_odom_ptr; + const auto & objects = *objects_ptr; + const auto & acc = *acc_ptr; + const auto traj_points = motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready if (traj_points.empty()) { @@ -506,14 +511,14 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // (1) with a proper label // (2) in front of ego // (3) not too far from trajectory - const auto target_obstacles = convertToObstacles(traj_points); + const auto target_obstacles = convertToObstacles(ego_odom, objects, traj_points); // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = - determineEgoBehaviorAgainstObstacles(traj_points, target_obstacles); + determineEgoBehaviorAgainstObstacles(ego_odom, objects, traj_points, target_obstacles); // 3. Create data for planning - const auto planner_data = createPlannerData(traj_points); + const auto planner_data = createPlannerData(ego_odom, acc, traj_points); // 4. Stop planning const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles); @@ -629,15 +634,16 @@ std::vector ObstacleCruisePlannerNode::createOneStepPolygons( } std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points) const { stop_watch_.tic(__func__); - const auto obj_stamp = rclcpp::Time(objects_sub_.getData().header.stamp); + const auto obj_stamp = rclcpp::Time(objects.header.stamp); const auto & p = behavior_determination_param_; std::vector target_obstacles; - for (const auto & predicted_object : objects_sub_.getData().objects) { + for (const auto & predicted_object : objects.objects) { const auto & object_id = tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); const auto & current_obstacle_pose = @@ -655,8 +661,7 @@ std::vector ObstacleCruisePlannerNode::convertToObstacles( } // 2. Check if the obstacle is in front of the ego. - const size_t ego_idx = - ego_nearest_param_.findIndex(traj_points, ego_odom_sub_.getData().pose.pose); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); const auto ego_to_obstacle_distance = calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); if (!ego_to_obstacle_distance) { @@ -745,14 +750,15 @@ bool ObstacleCruisePlannerNode::isFrontCollideObstacle( std::tuple, std::vector, std::vector> ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( + const Odometry & odometry, const PredictedObjects & objects, const std::vector & traj_points, const std::vector & obstacles) { stop_watch_.tic(__func__); // calculated decimated trajectory points and trajectory polygon - const auto decimated_traj_points = decimateTrajectoryPoints(traj_points); + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); const auto decimated_traj_polys = - createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose); + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); debug_data_ptr_->detection_polygons = decimated_traj_polys; // determine ego's behavior from stop, cruise and slow down @@ -777,14 +783,14 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( cruise_obstacles.push_back(*cruise_obstacle); continue; } - const auto stop_obstacle = - createStopObstacle(decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + const auto stop_obstacle = createStopObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); if (stop_obstacle) { stop_obstacles.push_back(*stop_obstacle); continue; } const auto slow_down_obstacle = - createSlowDownObstacle(decimated_traj_points, obstacle, precise_lat_dist); + createSlowDownObstacle(odometry, decimated_traj_points, obstacle, precise_lat_dist); if (slow_down_obstacle) { slow_down_obstacles.push_back(*slow_down_obstacle); continue; @@ -810,7 +816,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( slow_down_condition_counter_.removeCounterUnlessUpdated(); // Check target obstacles' consistency - checkConsistency(objects_sub_.getData().header.stamp, objects_sub_.getData(), stop_obstacles); + checkConsistency(objects.header.stamp, objects, stop_obstacles); // update previous obstacles prev_stop_obstacles_ = stop_obstacles; @@ -826,13 +832,12 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles( } std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( - const std::vector & traj_points) const + const Odometry & odometry, const std::vector & traj_points) const { const auto & p = behavior_determination_param_; // trim trajectory - const size_t ego_seg_idx = - ego_nearest_param_.findSegmentIndex(traj_points, ego_odom_sub_.getData().pose.pose); + const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(traj_points, odometry.pose.pose); const size_t traj_start_point_idx = ego_seg_idx; const auto trimmed_traj_points = std::vector(traj_points.begin() + traj_start_point_idx, traj_points.end()); @@ -1074,7 +1079,11 @@ ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise)); return collision_points; } @@ -1114,7 +1123,10 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), is_driving_forward_, collision_index, - vehicle_info_.vehicle_width_m + p.max_lat_margin_for_cruise, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), p.max_prediction_time_for_collision_check); if (collision_points.empty()) { // Ignore vehicle obstacles outside the trajectory without collision @@ -1151,8 +1163,9 @@ ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( } std::optional ObstacleCruisePlannerNode::createStopObstacle( - const std::vector & traj_points, const std::vector & traj_polys, - const Obstacle & obstacle, const double precise_lat_dist) const + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) const { const auto & p = behavior_determination_param_; const auto & object_id = obstacle.uuid.substr(0, 4); @@ -1190,7 +1203,11 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::vector collision_index; const auto collision_points = polygon_utils::getCollisionPoints( traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), - is_driving_forward_, collision_index); + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_stop)); if (collision_points.empty()) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1203,7 +1220,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const double collision_time_margin = - calcCollisionTimeMargin(collision_points, traj_points, is_driving_forward_); + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); if (p.collision_time_margin < collision_time_margin) { RCLCPP_INFO_EXPRESSION( get_logger(), enable_debug_info_, @@ -1216,7 +1233,7 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( // calculate collision points with trajectory with lateral stop margin const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, p.max_lat_margin_for_stop); + traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_stop); const auto collision_point = polygon_utils::getCollisionPoint( traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); @@ -1231,8 +1248,8 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( - const std::vector & traj_points, const Obstacle & obstacle, - const double precise_lat_dist) + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist) { const auto & object_id = obstacle.uuid.substr(0, 4); const auto & p = behavior_determination_param_; @@ -1286,7 +1303,7 @@ std::optional ObstacleCruisePlannerNode::createSlowDownObstacl // calculate collision points with trajectory with lateral stop margin // NOTE: For additional margin, hysteresis is not divided by two. const auto traj_polys_with_lat_margin = createOneStepPolygons( - traj_points, vehicle_info_, ego_odom_sub_.getData().pose.pose, + traj_points, vehicle_info_, odometry.pose.pose, p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); std::vector front_collision_polygons; @@ -1446,11 +1463,11 @@ bool ObstacleCruisePlannerNode::isObstacleCrossing( } double ObstacleCruisePlannerNode::calcCollisionTimeMargin( - const std::vector & collision_points, + const Odometry & odometry, const std::vector & collision_points, const std::vector & traj_points, const bool is_driving_forward) const { - const auto & ego_pose = ego_odom_sub_.getData().pose.pose; - const double ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; + const auto & ego_pose = odometry.pose.pose; + const double ego_vel = odometry.twist.twist.linear.x; const double time_to_reach_collision_point = [&]() { const double abs_ego_offset = is_driving_forward @@ -1477,14 +1494,15 @@ double ObstacleCruisePlannerNode::calcCollisionTimeMargin( } PlannerData ObstacleCruisePlannerNode::createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, const std::vector & traj_points) const { PlannerData planner_data; planner_data.current_time = now(); planner_data.traj_points = traj_points; - planner_data.ego_pose = ego_odom_sub_.getData().pose.pose; - planner_data.ego_vel = ego_odom_sub_.getData().twist.twist.linear.x; - planner_data.ego_acc = acc_sub_.getData().accel.accel.linear.x; + planner_data.ego_pose = odometry.pose.pose; + planner_data.ego_vel = odometry.twist.twist.linear.x; + planner_data.ego_acc = acc.accel.accel.linear.x; planner_data.is_driving_forward = is_driving_forward_; return planner_data; } diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index fa5a96b934f7a..1bd2de0bd985c 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -44,18 +44,18 @@ PointWithStamp calcNearestCollisionPoint( return collision_points.at(min_idx); } -// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon // calculation. std::optional>> getCollisionIndex( const std::vector & traj_points, const std::vector & traj_polygons, const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, - const Shape & object_shape, const double max_lat_dist = std::numeric_limits::max()) + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object_pose, object_shape); for (size_t i = 0; i < traj_polygons.size(); ++i) { const double approximated_dist = tier4_autoware_utils::calcDistance2d(traj_points.at(i).pose, object_pose); - if (approximated_dist > max_lat_dist) { + if (approximated_dist > max_dist) { continue; } diff --git a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp index 7b52f3860592e..c47404b45bb5d 100644 --- a/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp +++ b/planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -37,7 +36,9 @@ #include #include #include +#include #include +#include #include #include @@ -71,7 +72,6 @@ using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::OccupancyGrid; using nav_msgs::msg::Odometry; -using route_handler::RouteHandler; using sensor_msgs::msg::PointCloud2; using tf2_msgs::msg::TFMessage; using tier4_autoware_utils::createPoint; @@ -267,46 +267,6 @@ Scenario makeScenarioMsg(const std::string scenario) return scenario_msg; } -Pose createPoseFromLaneID(const lanelet::Id & lane_id) -{ - auto map_bin_msg = makeMapBinMsg(); - // create route_handler - auto route_handler = std::make_shared(); - route_handler->setMap(map_bin_msg); - - // get middle idx of the lanelet - const auto lanelet = route_handler->getLaneletsFromId(lane_id); - const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); - - // get middle position of the lanelet - geometry_msgs::msg::Point middle_pos; - middle_pos.x = center_line[middle_point_idx].x(); - middle_pos.y = center_line[middle_point_idx].y(); - - // get next middle position of the lanelet - geometry_msgs::msg::Point next_middle_pos; - next_middle_pos.x = center_line[middle_point_idx + 1].x(); - next_middle_pos.y = center_line[middle_point_idx + 1].y(); - - // calculate middle pose - geometry_msgs::msg::Pose middle_pose; - middle_pose.position = middle_pos; - const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos); - middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - - return middle_pose; -} - -Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id) -{ - Odometry current_odometry; - current_odometry.pose.pose = createPoseFromLaneID(lane_id); - current_odometry.header.frame_id = "map"; - - return current_odometry; -} - RouteSections combineConsecutiveRouteSections( const RouteSections & route_sections1, const RouteSections & route_sections2) { @@ -322,53 +282,6 @@ RouteSections combineConsecutiveRouteSections( return route_sections; } -// Function to create a route from given start and goal lanelet ids -// start pose and goal pose are set to the middle of the lanelet -LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) -{ - LaneletRoute route; - route.header.frame_id = "map"; - auto start_pose = createPoseFromLaneID(start_lane_id); - auto goal_pose = createPoseFromLaneID(goal_lane_id); - route.start_pose = start_pose; - route.goal_pose = goal_pose; - - auto map_bin_msg = makeMapBinMsg(); - // create route_handler - auto route_handler = std::make_shared(); - route_handler->setMap(map_bin_msg); - - LaneletRoute route_msg; - RouteSections route_sections; - lanelet::ConstLanelets all_route_lanelets; - - // Plan the path between checkpoints (start and goal poses) - lanelet::ConstLanelets path_lanelets; - if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) { - return route_msg; - } - - // Add all path_lanelets to all_route_lanelets - for (const auto & lane : path_lanelets) { - all_route_lanelets.push_back(lane); - } - // create local route sections - route_handler->setRouteLanelets(path_lanelets); - const auto local_route_sections = route_handler->createMapSegments(path_lanelets); - route_sections = combineConsecutiveRouteSections(route_sections, local_route_sections); - for (const auto & route_section : route_sections) { - for (const auto & primitive : route_section.primitives) { - std::cerr << "primitive: " << primitive.id << std::endl; - } - std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl; - } - route_handler->setRouteLanelets(all_route_lanelets); - route.segments = route_sections; - - route.allow_modification = false; - return route; -} - // this is for the test lanelet2_map.osm // file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5 LaneletRoute makeBehaviorNormalRoute() diff --git a/planning/planning_test_utils/package.xml b/planning/planning_test_utils/package.xml index 47540d6d3751f..b236a64f48da0 100644 --- a/planning/planning_test_utils/package.xml +++ b/planning/planning_test_utils/package.xml @@ -25,10 +25,8 @@ component_interface_utils lanelet2_extension lanelet2_io - motion_utils nav_msgs rclcpp - route_handler tf2_msgs tf2_ros tier4_api_msgs diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index d13c0536cf3c1..73bf99962ea82 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -205,54 +205,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false, const bool get_shoulder_lane = false) const; - /** - * @brief Searches the furthest linestring to the right side of the lanelet - * Only lanelet with same direction is considered - * @param the lanelet of interest - * @return right most linestring of the lane with same direction - */ - lanelet::ConstLineString3d getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the right side of the lanelet - * Used to search for road shoulders. Lane direction is ignored - * @param the lanelet of interest - * @return right most linestring - */ - lanelet::ConstLineString3d getRightMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the left side of the lanelet - * Only lanelet with same direction is considered - * @param the lanelet of interest - * @return left most linestring of the lane with same direction - */ - lanelet::ConstLineString3d getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Searches the furthest linestring to the left side of the lanelet - * Used to search for road shoulders. Lane direction is ignored - * @param the lanelet of interest - * @return left most linestring - */ - lanelet::ConstLineString3d getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root = false) const noexcept; - - /** - * @brief Return furthest linestring on both side of the lanelet - * @param the lanelet of interest - * @param (optional) search furthest right side - * @param (optional) search furthest left side - * @param (optional) include opposite lane as well - * @return right and left linestrings - */ - lanelet::ConstLineStrings3d getFurthestLinestring( - const lanelet::ConstLanelet & lanelet, bool is_right = true, bool is_left = true, - bool is_opposite = true, bool enable_same_root = false) const noexcept; - /** * Retrieves a sequence of lanelets before the given lanelet. * The total length of retrieved lanelet sequence at least given length. Returned lanelet sequence @@ -276,18 +228,6 @@ class RouteHandler int getNumLaneToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; - /** - * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return - * the distance to the preferred lane from the give lane. - * The total distance is computed from the front point of the centerline of the given lane to - * the front point of the preferred lane. - * @param lanelet lanelet to query - * @param direction change direction - * @return number of lanes from input to the preferred lane - */ - double getTotalLateralDistanceToPreferredLane( - const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; - /** * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return * the distance to the preferred lane from the give lane. @@ -321,11 +261,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const Pose & pose, const double backward_distance = std::numeric_limits::max(), const double forward_distance = std::numeric_limits::max()) const; - lanelet::ConstLanelets getCheckTargetLanesFromPath( - const PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, - const double check_length) const; - lanelet::routing::RelationType getRelation( - const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const; bool isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const; bool isRouteLanelet(const lanelet::ConstLanelet & lanelet) const; bool isRoadLanelet(const lanelet::ConstLanelet & lanelet) const; @@ -339,14 +274,9 @@ class RouteHandler const lanelet::ConstLanelets & lanelets, const Direction direction = Direction::NONE) const; std::optional getLaneChangeTargetExceptPreferredLane( const lanelet::ConstLanelets & lanelets, const Direction direction) const; - bool getRightLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; - bool getLeftLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; std::optional getPullOverTarget(const Pose & goal_pose) const; std::optional getPullOutStartLane( const Pose & pose, const double vehicle_width) const; - double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const; lanelet::ConstLanelets getRoadLaneletsAtPose(const Pose & pose) const; std::optional getLeftShoulderLanelet( const lanelet::ConstLanelet & lanelet) const; @@ -354,7 +284,6 @@ class RouteHandler const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getShoulderLaneletsAtPose(const Pose & pose) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; - bool isPreferredLane(const lanelet::ConstLanelet & lanelet) const; private: // MUST @@ -385,8 +314,6 @@ class RouteHandler lanelet::ConstLanelets getMainLanelets(const lanelet::ConstLanelets & path_lanelets) const; // for lanelet - bool isInTargetLane(const PoseStamped & pose, const lanelet::ConstLanelets & target) const; - bool isInPreferredLane(const PoseStamped & pose) const; bool isBijectiveConnection( const lanelet::ConstLanelets & lanelet_section1, const lanelet::ConstLanelets & lanelet_section2) const; @@ -419,17 +346,13 @@ class RouteHandler const double min_length = std::numeric_limits::max()) const; lanelet::ConstLanelets getPreviousLaneletSequence( const lanelet::ConstLanelets & lanelet_sequence) const; - lanelet::ConstLanelets getLaneChangeTargetLanes(const Pose & pose) const; lanelet::ConstLanelets getLaneSequenceUpTo(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneSequenceAfter(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getLaneSequence(const lanelet::ConstLanelet & lanelet) const; lanelet::ConstLanelets getNeighborsWithinRoute(const lanelet::ConstLanelet & lanelet) const; - std::vector getLaneSection(const lanelet::ConstLanelet & lanelet) const; - lanelet::ConstLanelets getNextLaneSequence(const lanelet::ConstLanelets & lane_sequence) const; // for path - PathWithLaneId updatePathTwist(const PathWithLaneId & path) const; /** * @brief Checks if a path has a no_drivable_lane or not * @param path lanelet path diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index 589a98d7d5ab4..4dd559b9bbc98 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -573,44 +573,26 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequenceUpTo( lanelet::ConstLanelet current_lanelet = lanelet; double length = 0; + lanelet::ConstLanelets previous_lanelets; while (rclcpp::ok() && length < min_length) { - lanelet::ConstLanelets candidate_lanelets; - if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { - if (only_route_lanes) { - break; - } - const auto prev_lanes = getPreviousLanelets(current_lanelet); - if (prev_lanes.empty()) { - break; - } - candidate_lanelets = prev_lanes; + previous_lanelets.clear(); + if (!getPreviousLaneletsWithinRoute(current_lanelet, &previous_lanelets)) { + if (only_route_lanes) break; + const auto previous_lanelets = getPreviousLanelets(current_lanelet); + if (previous_lanelets.empty()) break; } // loop check - if (std::any_of( - candidate_lanelets.begin(), candidate_lanelets.end(), - [lanelet](auto & prev_llt) { return lanelet.id() == prev_llt.id(); })) { - break; - } - - // If lanelet_sequence_backward with input lanelet contains all candidate lanelets, - // break the loop. - if (std::all_of( - candidate_lanelets.begin(), candidate_lanelets.end(), - [lanelet_sequence_backward, lanelet](auto & prev_llt) { - return std::any_of( - lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), - [prev_llt, lanelet](auto & llt) { - return (llt.id() == prev_llt.id() || lanelet.id() == prev_llt.id()); - }); - })) { + if (std::any_of(previous_lanelets.begin(), previous_lanelets.end(), [lanelet](auto & prev_llt) { + return lanelet.id() == prev_llt.id(); + })) { break; } - for (const auto & prev_lanelet : candidate_lanelets) { + for (const auto & prev_lanelet : previous_lanelets) { if (std::any_of( lanelet_sequence_backward.begin(), lanelet_sequence_backward.end(), - [prev_lanelet, lanelet](auto & llt) { - return (llt.id() == prev_lanelet.id() || lanelet.id() == prev_lanelet.id()); + [prev_lanelet, lanelet](auto & backward) { + return (backward.id() == prev_lanelet.id()); })) { continue; } @@ -680,14 +662,13 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( const lanelet::ConstLanelet & lanelet, const Pose & current_pose, const double backward_distance, const double forward_distance, const bool only_route_lanes) const { - lanelet::ConstLanelets lanelet_sequence; if (only_route_lanes && !exists(route_lanelets_, lanelet)) { - return lanelet_sequence; + return {}; } lanelet::ConstLanelets lanelet_sequence_forward = getLaneletSequenceAfter(lanelet, forward_distance, only_route_lanes); - const lanelet::ConstLanelets lanelet_sequence_backward = std::invoke([&]() { + lanelet::ConstLanelets lanelet_sequence = std::invoke([&]() { const auto arc_coordinate = lanelet::utils::getArcCoordinates({lanelet}, current_pose); if (arc_coordinate.length < backward_distance) { return getLaneletSequenceUpTo(lanelet, backward_distance, only_route_lanes); @@ -696,17 +677,15 @@ lanelet::ConstLanelets RouteHandler::getLaneletSequence( }); // loop check - if (!lanelet_sequence_forward.empty() && !lanelet_sequence_backward.empty()) { - if (lanelet_sequence_backward.back().id() == lanelet_sequence_forward.front().id()) { + if (!lanelet_sequence_forward.empty() && !lanelet_sequence.empty()) { + if (lanelet_sequence.back().id() == lanelet_sequence_forward.front().id()) { return lanelet_sequence_forward; } } - lanelet_sequence.insert( - lanelet_sequence.end(), lanelet_sequence_backward.begin(), lanelet_sequence_backward.end()); lanelet_sequence.push_back(lanelet); - lanelet_sequence.insert( - lanelet_sequence.end(), lanelet_sequence_forward.begin(), lanelet_sequence_forward.end()); - + std::move( + lanelet_sequence_forward.begin(), lanelet_sequence_forward.end(), + std::back_inserter(lanelet_sequence)); return lanelet_sequence; } @@ -1324,98 +1303,6 @@ lanelet::ConstLanelet RouteHandler::getMostLeftLanelet( return lanelet; } -lanelet::ConstLineString3d RouteHandler::getRightMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getRightLanelet(lanelet, enable_same_root); - - if (same) { - return getRightMostSameDirectionLinestring(same.value(), enable_same_root); - } - - return lanelet.rightBound(); -} - -lanelet::ConstLineString3d RouteHandler::getRightMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - const auto & same = getRightLanelet(lanelet, enable_same_root); - const auto & opposite = getRightOppositeLanelets(lanelet); - if (!same && opposite.empty()) { - return lanelet.rightBound(); - } - - if (same) { - return getRightMostLinestring(same.value(), enable_same_root); - } - - if (!opposite.empty()) { - return getLeftMostLinestring(lanelet::ConstLanelet(opposite.front()), false); - } - - return lanelet.rightBound(); -} - -lanelet::ConstLineString3d RouteHandler::getLeftMostSameDirectionLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); - - if (same) { - return getLeftMostSameDirectionLinestring(same.value(), enable_same_root); - } - - return lanelet.leftBound(); -} - -lanelet::ConstLineString3d RouteHandler::getLeftMostLinestring( - const lanelet::ConstLanelet & lanelet, const bool enable_same_root) const noexcept -{ - // recursively compute the width of the lanes - const auto & same = getLeftLanelet(lanelet, enable_same_root); - const auto & opposite = getLeftOppositeLanelets(lanelet); - if (!same && opposite.empty()) { - return lanelet.leftBound(); - } - - if (same) { - return getLeftMostLinestring(same.value(), enable_same_root); - } - - if (!opposite.empty()) { - return getRightMostLinestring(lanelet::ConstLanelet(opposite.front()), false); - } - - return lanelet.leftBound(); -} - -lanelet::ConstLineStrings3d RouteHandler::getFurthestLinestring( - const lanelet::ConstLanelet & lanelet, bool is_right, bool is_left, bool is_opposite, - bool enable_same_root) const noexcept -{ - lanelet::ConstLineStrings3d linestrings; - linestrings.reserve(2); - - if (is_right && is_opposite) { - linestrings.emplace_back(getRightMostLinestring(lanelet, enable_same_root)); - } else if (is_right && !is_opposite) { - linestrings.emplace_back(getRightMostSameDirectionLinestring(lanelet, enable_same_root)); - } else { - linestrings.emplace_back(lanelet.rightBound()); - } - - if (is_left && is_opposite) { - linestrings.emplace_back(getLeftMostLinestring(lanelet, enable_same_root)); - } else if (is_left && !is_opposite) { - linestrings.emplace_back(getLeftMostSameDirectionLinestring(lanelet, enable_same_root)); - } else { - linestrings.emplace_back(lanelet.leftBound()); - } - return linestrings; -} - std::vector RouteHandler::getPrecedingLaneletSequence( const lanelet::ConstLanelet & lanelet, const double length, const lanelet::ConstLanelets & exclude_lanelets) const @@ -1486,46 +1373,6 @@ std::optional RouteHandler::getLaneChangeTargetExceptPref return std::nullopt; } -bool RouteHandler::getRightLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const -{ - for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); - - // Get right lanelet if preferred lane is on the left - if (num >= 0) { - if (!!routing_graph_ptr_->right(lanelet)) { - const auto right_lanelet = routing_graph_ptr_->right(lanelet); - *target_lanelet = right_lanelet.value(); - return true; - } - } - } - - *target_lanelet = lanelets.front(); - return false; -} - -bool RouteHandler::getLeftLaneChangeTargetExceptPreferredLane( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const -{ - for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); - - // Get left lanelet if preferred lane is on the right - if (num <= 0) { - if (!!routing_graph_ptr_->left(lanelet)) { - const auto left_lanelet = routing_graph_ptr_->left(lanelet); - *target_lanelet = left_lanelet.value(); - return true; - } - } - } - - *target_lanelet = lanelets.front(); - return false; -} - std::optional RouteHandler::getPullOverTarget(const Pose & goal_pose) const { const lanelet::BasicPoint2d p(goal_pose.position.x, goal_pose.position.y); @@ -1589,13 +1436,6 @@ int RouteHandler::getNumLaneToPreferredLane( return 0; // TODO(Horibe) check if return 0 is appropriate. } -double RouteHandler::getTotalLateralDistanceToPreferredLane( - const lanelet::ConstLanelet & lanelet, const Direction direction) const -{ - const auto intervals = getLateralIntervalsToPreferredLane(lanelet, direction); - return std::accumulate(intervals.begin(), intervals.end(), 0); -} - std::vector RouteHandler::getLateralIntervalsToPreferredLane( const lanelet::ConstLanelet & lanelet, const Direction direction) const { @@ -1650,30 +1490,6 @@ std::vector RouteHandler::getLateralIntervalsToPreferredLane( return {}; } -bool RouteHandler::isPreferredLane(const lanelet::ConstLanelet & lanelet) const -{ - return exists(preferred_lanelets_, lanelet); -} - -bool RouteHandler::isInPreferredLane(const PoseStamped & pose) const -{ - lanelet::ConstLanelet lanelet; - if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { - return false; - } - return exists(preferred_lanelets_, lanelet); -} - -bool RouteHandler::isInTargetLane( - const PoseStamped & pose, const lanelet::ConstLanelets & target) const -{ - lanelet::ConstLanelet lanelet; - if (!getClosestLaneletWithinRoute(pose.pose, &lanelet)) { - return false; - } - return exists(target, lanelet); -} - PathWithLaneId RouteHandler::getCenterLinePath( const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact) const @@ -1749,139 +1565,6 @@ PathWithLaneId RouteHandler::getCenterLinePath( return reference_path; } -PathWithLaneId RouteHandler::updatePathTwist(const PathWithLaneId & path) const -{ - PathWithLaneId updated_path = path; - for (auto & point : updated_path.points) { - const auto id = point.lane_ids.at(0); - const auto llt = lanelet_map_ptr_->laneletLayer.get(id); - const auto limit = traffic_rules_ptr_->speedLimit(llt); - point.point.longitudinal_velocity_mps = static_cast(limit.speedLimit.value()); - } - return updated_path; -} - -lanelet::ConstLanelets RouteHandler::getLaneChangeTargetLanes(const Pose & pose) const -{ - lanelet::ConstLanelet lanelet; - lanelet::ConstLanelets target_lanelets; - if (!getClosestLaneletWithinRoute(pose, &lanelet)) { - return target_lanelets; - } - - const int num = getNumLaneToPreferredLane(lanelet); - if (num < 0) { - const auto right_lanelet = (!!routing_graph_ptr_->right(lanelet)) - ? routing_graph_ptr_->right(lanelet) - : routing_graph_ptr_->adjacentRight(lanelet); - target_lanelets = getLaneletSequence(right_lanelet.value()); - } - if (num > 0) { - const auto left_lanelet = (!!routing_graph_ptr_->left(lanelet)) - ? routing_graph_ptr_->left(lanelet) - : routing_graph_ptr_->adjacentLeft(lanelet); - target_lanelets = getLaneletSequence(left_lanelet.value()); - } - return target_lanelets; -} - -double RouteHandler::getLaneChangeableDistance( - const Pose & current_pose, const Direction & direction) const -{ - lanelet::ConstLanelet current_lane; - if (!getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { - return 0; - } - - // get lanelets after current lane - auto lanelet_sequence = getLaneletSequenceAfter(current_lane); - lanelet_sequence.insert(lanelet_sequence.begin(), current_lane); - - double accumulated_distance = 0; - for (const auto & lane : lanelet_sequence) { - lanelet::ConstLanelet target_lane; - if (direction == Direction::RIGHT) { - if (!getRightLaneletWithinRoute(lane, &target_lane)) { - break; - } - } - if (direction == Direction::LEFT) { - if (!getLeftLaneletWithinRoute(lane, &target_lane)) { - break; - } - } - double lane_length = lanelet::utils::getLaneletLength3d(lane); - - // overwrite goal because lane change must be finished before reaching goal - if (isInGoalRouteSection(lane)) { - const auto goal_position = lanelet::utils::conversion::toLaneletPoint(getGoalPose().position); - const auto goal_arc_coordinates = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(lane.centerline()), lanelet::utils::to2D(goal_position).basicPoint()); - lane_length = std::min(goal_arc_coordinates.length, lane_length); - } - - // subtract distance up to current position for first lane - if (lane == current_lane) { - const auto current_position = - lanelet::utils::conversion::toLaneletPoint(current_pose.position); - const auto arc_coordinate = lanelet::geometry::toArcCoordinates( - lanelet::utils::to2D(lane.centerline()), - lanelet::utils::to2D(current_position).basicPoint()); - lane_length = std::max(lane_length - arc_coordinate.length, 0.0); - } - accumulated_distance += lane_length; - } - return accumulated_distance; -} - -lanelet::ConstLanelets RouteHandler::getCheckTargetLanesFromPath( - const PathWithLaneId & path, const lanelet::ConstLanelets & target_lanes, - const double check_length) const -{ - std::vector target_lane_ids; - target_lane_ids.reserve(target_lanes.size()); - for (const auto & llt : target_lanes) { - target_lane_ids.push_back(llt.id()); - } - - // find first lanelet in target lanes along path - int64_t root_lane_id = lanelet::InvalId; - for (const auto & point : path.points) { - for (const auto & lane_id : point.lane_ids) { - if (exists(target_lane_ids, lane_id)) { - root_lane_id = lane_id; - } - } - } - // return emtpy lane if failed to find root lane_id - if (root_lane_id == lanelet::InvalId) { - return target_lanes; - } - lanelet::ConstLanelet root_lanelet; - for (const auto & llt : target_lanes) { - if (llt.id() == root_lane_id) { - root_lanelet = llt; - } - } - - const auto sequences = lanelet::utils::query::getPrecedingLaneletSequences( - routing_graph_ptr_, root_lanelet, check_length); - lanelet::ConstLanelets check_lanelets; - for (const auto & sequence : sequences) { - for (const auto & llt : sequence) { - if (!lanelet::utils::contains(check_lanelets, llt)) { - check_lanelets.push_back(llt); - } - } - } - for (const auto & llt : target_lanes) { - if (!lanelet::utils::contains(check_lanelets, llt)) { - check_lanelets.push_back(llt); - } - } - return check_lanelets; -} - bool RouteHandler::isMapMsgReady() const { return is_map_msg_ready_; @@ -1908,41 +1591,6 @@ lanelet::LaneletMapPtr RouteHandler::getLaneletMapPtr() const return lanelet_map_ptr_; } -lanelet::routing::RelationType RouteHandler::getRelation( - const lanelet::ConstLanelet & prev_lane, const lanelet::ConstLanelet & next_lane) const -{ - if (prev_lane == next_lane) { - return lanelet::routing::RelationType::None; - } - const auto & relation = routing_graph_ptr_->routingRelation(prev_lane, next_lane); - if (relation) { - return relation.value(); - } - - // check if lane change extends across multiple lanes - const auto shortest_path = routing_graph_ptr_->shortestPath(prev_lane, next_lane); - if (shortest_path) { - auto prev_llt = shortest_path->front(); - for (const auto & llt : shortest_path.value()) { - if (prev_llt == llt) { - continue; - } - const auto & relation = routing_graph_ptr_->routingRelation(prev_llt, llt); - if (!relation) { - continue; - } - if ( - relation.value() == lanelet::routing::RelationType::Left || - relation.value() == lanelet::routing::RelationType::Right) { - return relation.value(); - } - prev_llt = llt; - } - } - - return lanelet::routing::RelationType::None; -} - bool RouteHandler::isShoulderLanelet(const lanelet::ConstLanelet & lanelet) const { return lanelet.hasAttribute(lanelet::AttributeName::Subtype) && @@ -2012,8 +1660,9 @@ lanelet::ConstLanelets RouteHandler::getLaneSequenceUpTo( } lanelet::ConstLanelet current_lanelet = lanelet; + lanelet::ConstLanelets candidate_lanelets; while (rclcpp::ok()) { - lanelet::ConstLanelets candidate_lanelets; + candidate_lanelets.clear(); if (!getPreviousLaneletsWithinRoute(current_lanelet, &candidate_lanelets)) { break; } @@ -2100,33 +1749,6 @@ lanelet::ConstLanelets RouteHandler::getNeighborsWithinRoute( return neighbors_within_route; } -std::vector RouteHandler::getLaneSection( - const lanelet::ConstLanelet & lanelet) const -{ - const lanelet::ConstLanelets neighbors = getNeighborsWithinRoute(lanelet); - std::vector lane_section; - lane_section.reserve(neighbors.size()); - for (const auto & llt : neighbors) { - lane_section.push_back(getLaneSequence(llt)); - } - return lane_section; -} - -lanelet::ConstLanelets RouteHandler::getNextLaneSequence( - const lanelet::ConstLanelets & lane_sequence) const -{ - lanelet::ConstLanelets next_lane_sequence; - if (lane_sequence.empty()) { - return next_lane_sequence; - } - const auto & final_lanelet = lane_sequence.back(); - lanelet::ConstLanelet next_lanelet; - if (!getNextLaneletWithinRoute(final_lanelet, &next_lanelet)) { - return next_lane_sequence; - } - return getLaneSequence(next_lanelet); -} - bool RouteHandler::planPathLaneletsBetweenCheckpoints( const Pose & start_checkpoint, const Pose & goal_checkpoint, lanelet::ConstLanelets * path_lanelets, const bool consider_no_drivable_lanes) const diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index 8d6e8c8fc1c7c..7180812c7500b 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -56,6 +56,7 @@ class RTCInterface const UUID & uuid, const bool safe, const uint8_t state, const double start_distance, const double finish_distance, const rclcpp::Time & stamp); void removeCooperateStatus(const UUID & uuid); + void removeExpiredCooperateStatus(); void clearCooperateStatus(); bool isActivated(const UUID & uuid) const; bool isRegistered(const UUID & uuid) const; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index c7afea57afce3..623f899f55c70 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -265,6 +265,18 @@ void RTCInterface::removeStoredCommand(const UUID & uuid) } } +void RTCInterface::removeExpiredCooperateStatus() +{ + std::lock_guard lock(mutex_); + const auto itr = std::remove_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [](const auto & status) { + return (rclcpp::Clock{RCL_ROS_TIME}.now() - status.stamp).seconds() > 10.0; + }); + + registered_status_.statuses.erase(itr, registered_status_.statuses.end()); +} + void RTCInterface::clearCooperateStatus() { std::lock_guard lock(mutex_); diff --git a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md index 61f65bc40f32d..e87023ef00149 100644 --- a/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md +++ b/sensing/pointcloud_preprocessor/docs/ring-outlier-filter.md @@ -10,6 +10,40 @@ A method of operating scan in chronological order and removing noise based on th ![ring_outlier_filter](./image/outlier_filter-ring.drawio.svg) +Another feature of this node is that it calculates visibility score based on outlier pointcloud and publish score as a topic. + +### visibility score calculation algorithm + +The pointcloud is divided into vertical bins (rings) and horizontal bins (azimuth divisions). +The algorithm starts by splitting the input point cloud into separate rings based on the ring value of each point. Then, for each ring, it iterates through the points and calculates the frequency of points within each horizontal bin. The frequency is determined by incrementing a counter for the corresponding bin based on the point's azimuth value. +The frequency values are stored in a frequency image matrix, where each cell represents a specific ring and azimuth bin. After calculating the frequency image, the algorithm applies a noise threshold to create a binary image. Points with frequency values above the noise threshold are considered valid, while points below the threshold are considered noise. +Finally, the algorithm calculates the visibility score by counting the number of non-zero pixels in the frequency image and dividing it by the total number of pixels (vertical bins multiplied by horizontal bins). + +```plantuml +@startuml +start + +:Convert input point cloud to PCL format; + +:Initialize vertical and horizontal bins; + +:Split point cloud into rings; + +while (For each ring) is (not empty) + :Calculate frequency of points in each azimuth bin; + :Update frequency image matrix; +endwhile + +:Apply noise threshold to create binary image; + +:Count non-zero pixels in frequency image; + +:Calculate visibility score as complement of filled pixel ratio; + +stop +@enduml +``` + ## Inputs / Outputs This implementation inherits `pointcloud_preprocessor::Filter` class, please refer [README](../README.md). @@ -22,14 +56,20 @@ This implementation inherits `pointcloud_preprocessor::Filter` class, please ref ### Core Parameters -| Name | Type | Default Value | Description | -| ---------------------------- | ------- | ------------- | -------------------------------------------------------------------------------------------------------- | -| `distance_ratio` | double | 1.03 | | -| `object_length_threshold` | double | 0.1 | | -| `num_points_threshold` | int | 4 | | -| `max_rings_num` | uint_16 | 128 | | -| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | -| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud. Due to performance concerns, please set to false during experiments. | +| Name | Type | Default Value | Description | +| ---------------------------- | ------- | ------------- | ----------------------------------------------------------------------------------------------------------------------------- | +| `distance_ratio` | double | 1.03 | | +| `object_length_threshold` | double | 0.1 | | +| `num_points_threshold` | int | 4 | | +| `max_rings_num` | uint_16 | 128 | | +| `max_points_num_per_ring` | size_t | 4000 | Set this value large enough such that `HFoV / resolution < max_points_num_per_ring` | +| `publish_outlier_pointcloud` | bool | false | Flag to publish outlier pointcloud and visibility score. Due to performance concerns, please set to false during experiments. | +| `min_azimuth_deg` | float | 0.0 | The left limit of azimuth for visibility score calculation | +| `max_azimuth_deg` | float | 360.0 | The right limit of azimuth for visibility score calculation | +| `max_distance` | float | 12.0 | The limit distance for visibility score calculation | +| `vertical_bins` | int | 128 | The number of vertical bin for visibility histogram | +| `horizontal_bins` | int | 36 | The number of horizontal bin for visibility histogram | +| `noise_threshold` | int | 2 | The threshold value for distinguishing noise from valid points in the frequency image | ## Assumptions / Known limits diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index c871f1acbe5b9..55ba79aac4593 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -19,9 +19,17 @@ #include "pointcloud_preprocessor/filter.hpp" #include "pointcloud_preprocessor/transform_info.hpp" +#include #include +#if __has_include() +#include +#else +#include +#endif + #include +#include #include #include @@ -42,6 +50,8 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output, const TransformInfo & transform_info); + rclcpp::Publisher::SharedPtr visibility_pub_; + private: /** \brief publisher of excluded pointcloud for debug reason. **/ rclcpp::Publisher::SharedPtr outlier_pointcloud_publisher_; @@ -53,6 +63,15 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter size_t max_points_num_per_ring_; bool publish_outlier_pointcloud_; + // for visibility score + int noise_threshold_; + int vertical_bins_; + int horizontal_bins_; + + float min_azimuth_deg_; + float max_azimuth_deg_; + float max_distance_; + /** \brief Parameter service callback result : needed to be hold */ OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -77,6 +96,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter void setUpPointCloudFormat( const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, size_t num_fields); + float calculateVisibilityScore(const PointCloud2 & input); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/package.xml b/sensing/pointcloud_preprocessor/package.xml index 9a1eae786c379..47fa39e7fcda2 100644 --- a/sensing/pointcloud_preprocessor/package.xml +++ b/sensing/pointcloud_preprocessor/package.xml @@ -22,7 +22,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_geometry autoware_point_types cgal cv_bridge diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 762c62dc03b39..ada7d2616ed38 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -14,16 +14,16 @@ #include "pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp" -#include "autoware_auto_geometry/common_3d.hpp" +#include "autoware_point_types/types.hpp" #include -#include - #include #include namespace pointcloud_preprocessor { +using autoware_point_types::PointXYZIRADRT; + RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options) : Filter("RingOutlierFilter", options) { @@ -35,6 +35,8 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions debug_publisher_ = std::make_unique(this, "ring_outlier_filter"); outlier_pointcloud_publisher_ = this->create_publisher("debug/ring_outlier_filter", 1); + visibility_pub_ = create_publisher( + "ring_outlier_filter/debug/visibility", rclcpp::SensorDataQoS()); stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } @@ -50,6 +52,13 @@ RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions static_cast(declare_parameter("max_points_num_per_ring", 4000)); publish_outlier_pointcloud_ = static_cast(declare_parameter("publish_outlier_pointcloud", false)); + + min_azimuth_deg_ = static_cast(declare_parameter("min_azimuth_deg", 0.0)); + max_azimuth_deg_ = static_cast(declare_parameter("max_azimuth_deg", 360.0)); + max_distance_ = static_cast(declare_parameter("max_distance", 12.0)); + vertical_bins_ = static_cast(declare_parameter("vertical_bins", 128)); + horizontal_bins_ = static_cast(declare_parameter("horizontal_bins", 36)); + noise_threshold_ = static_cast(declare_parameter("noise_threshold", 2)); } using std::placeholders::_1; @@ -73,13 +82,7 @@ void RingOutlierFilterComponent::faster_filter( output.data.resize(output.point_step * input->width); size_t output_size = 0; - // Set up the noise points cloud, if noise points are to be published. - PointCloud2 outlier_points; - size_t outlier_points_size = 0; - if (publish_outlier_pointcloud_) { - outlier_points.point_step = sizeof(PointXYZI); - outlier_points.data.resize(outlier_points.point_step * input->width); - } + pcl::PointCloud::Ptr outlier_pcl(new pcl::PointCloud); const auto ring_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Ring)).offset; @@ -89,6 +92,10 @@ void RingOutlierFilterComponent::faster_filter( input->fields.at(static_cast(autoware_point_types::PointIndex::Distance)).offset; const auto intensity_offset = input->fields.at(static_cast(autoware_point_types::PointIndex::Intensity)).offset; + const auto return_type_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::ReturnType)).offset; + const auto time_stamp_offset = + input->fields.at(static_cast(autoware_point_types::PointIndex::TimeStamp)).offset; std::vector> ring2indices; ring2indices.reserve(max_rings_num_); @@ -163,24 +170,32 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i <= walk_last_idx; i++) { - auto outlier_ptr = - reinterpret_cast(&outlier_points.data[outlier_points_size]); + PointXYZIRADRT outlier_point; auto input_ptr = - reinterpret_cast(&input->data[indices[walk_first_idx]]); + reinterpret_cast(&input->data[indices[walk_first_idx]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = *reinterpret_cast( - &input->data[indices[walk_first_idx] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.intensity = *reinterpret_cast( + &input->data[indices[walk_first_idx] + intensity_offset]); + outlier_point.ring = *reinterpret_cast( + &input->data[indices[walk_first_idx] + ring_offset]); + outlier_point.azimuth = *reinterpret_cast( + &input->data[indices[walk_first_idx] + azimuth_offset]); + outlier_point.distance = *reinterpret_cast( + &input->data[indices[walk_first_idx] + distance_offset]); + outlier_point.return_type = *reinterpret_cast( + &input->data[indices[walk_first_idx] + return_type_offset]); + outlier_point.time_stamp = *reinterpret_cast( + &input->data[indices[walk_first_idx] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } @@ -213,21 +228,31 @@ void RingOutlierFilterComponent::faster_filter( } } else if (publish_outlier_pointcloud_) { for (int i = walk_first_idx; i < walk_last_idx; i++) { - auto outlier_ptr = reinterpret_cast(&outlier_points.data[outlier_points_size]); - auto input_ptr = reinterpret_cast(&input->data[indices[i]]); + PointXYZIRADRT outlier_point; + + auto input_ptr = reinterpret_cast(&input->data[indices[i]]); if (transform_info.need_transform) { Eigen::Vector4f p(input_ptr->x, input_ptr->y, input_ptr->z, 1); p = transform_info.eigen_transform * p; - outlier_ptr->x = p[0]; - outlier_ptr->y = p[1]; - outlier_ptr->z = p[2]; + outlier_point.x = p[0]; + outlier_point.y = p[1]; + outlier_point.z = p[2]; } else { - *outlier_ptr = *input_ptr; + outlier_point = *input_ptr; } - const float & intensity = + outlier_point.intensity = *reinterpret_cast(&input->data[indices[i] + intensity_offset]); - outlier_ptr->intensity = intensity; - outlier_points_size += outlier_points.point_step; + outlier_point.ring = + *reinterpret_cast(&input->data[indices[i] + ring_offset]); + outlier_point.azimuth = + *reinterpret_cast(&input->data[indices[i] + azimuth_offset]); + outlier_point.distance = + *reinterpret_cast(&input->data[indices[i] + distance_offset]); + outlier_point.return_type = + *reinterpret_cast(&input->data[indices[i] + return_type_offset]); + outlier_point.time_stamp = + *reinterpret_cast(&input->data[indices[i] + time_stamp_offset]); + outlier_pcl->push_back(outlier_point); } } } @@ -235,8 +260,15 @@ void RingOutlierFilterComponent::faster_filter( setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); - outlier_pointcloud_publisher_->publish(outlier_points); + PointCloud2 outlier; + pcl::toROSMsg(*outlier_pcl, outlier); + outlier.header = input->header; + outlier_pointcloud_publisher_->publish(outlier); + + tier4_debug_msgs::msg::Float32Stamped visibility_msg; + visibility_msg.data = calculateVisibilityScore(outlier); + visibility_msg.stamp = input->header.stamp; + visibility_pub_->publish(visibility_msg); } // add processing time for debug @@ -288,6 +320,24 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba RCLCPP_DEBUG( get_logger(), "Setting new publish_outlier_pointcloud to: %d.", publish_outlier_pointcloud_); } + if (get_param(p, "vertical_bins", vertical_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new vertical_bins to: %d.", vertical_bins_); + } + if (get_param(p, "horizontal_bins", horizontal_bins_)) { + RCLCPP_DEBUG(get_logger(), "Setting new horizontal_bins to: %d.", horizontal_bins_); + } + if (get_param(p, "noise_threshold", noise_threshold_)) { + RCLCPP_DEBUG(get_logger(), "Setting new noise_threshold to: %d.", noise_threshold_); + } + if (get_param(p, "max_azimuth_deg", max_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_azimuth_deg to: %f.", max_azimuth_deg_); + } + if (get_param(p, "min_azimuth_deg", min_azimuth_deg_)) { + RCLCPP_DEBUG(get_logger(), "Setting new min_azimuth_deg to: %f.", min_azimuth_deg_); + } + if (get_param(p, "max_distance", max_distance_)) { + RCLCPP_DEBUG(get_logger(), "Setting new max_distance to: %f.", max_distance_); + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -319,6 +369,61 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); } +float RingOutlierFilterComponent::calculateVisibilityScore( + const sensor_msgs::msg::PointCloud2 & input) +{ + pcl::PointCloud::Ptr input_cloud(new pcl::PointCloud); + pcl::fromROSMsg(input, *input_cloud); + + const uint32_t vertical_bins = vertical_bins_; + const uint32_t horizontal_bins = horizontal_bins_; + const float max_azimuth = max_azimuth_deg_ * 100.0; + const float min_azimuth = min_azimuth_deg_ * 100.0; + + const uint32_t horizontal_resolution = + static_cast((max_azimuth - min_azimuth) / horizontal_bins); + + std::vector> ring_point_clouds(vertical_bins); + cv::Mat frequency_image(cv::Size(horizontal_bins, vertical_bins), CV_8UC1, cv::Scalar(0)); + + // Split points into rings + for (const auto & point : input_cloud->points) { + ring_point_clouds.at(point.ring).push_back(point); + } + + // Calculate frequency for each bin in each ring + for (const auto & ring_points : ring_point_clouds) { + if (ring_points.empty()) continue; + + const uint ring_id = ring_points.front().ring; + std::vector frequency_in_ring(horizontal_bins, 0); + + for (const auto & point : ring_points.points) { + if (point.azimuth < min_azimuth || point.azimuth >= max_azimuth) continue; + if (point.distance >= max_distance_) continue; + + const uint bin_index = + static_cast((point.azimuth - min_azimuth) / horizontal_resolution); + + frequency_in_ring[bin_index]++; + frequency_in_ring[bin_index] = + std::min(frequency_in_ring[bin_index], 255); // Ensure value is within uchar range + + frequency_image.at(ring_id, bin_index) = + static_cast(frequency_in_ring[bin_index]); + } + } + + cv::Mat binary_image; + cv::inRange(frequency_image, noise_threshold_, 255, binary_image); + + const int num_pixels = cv::countNonZero(frequency_image); + const float num_filled_pixels = + static_cast(num_pixels) / static_cast(vertical_bins * horizontal_bins); + + return 1.0f - num_filled_pixels; +} + } // namespace pointcloud_preprocessor #include diff --git a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt index 35cb599995b31..c14f71571d272 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt +++ b/system/default_ad_api_helpers/ad_api_adaptors/CMakeLists.txt @@ -4,12 +4,21 @@ project(ad_api_adaptors) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(initial_pose_adaptor +ament_auto_add_library(${PROJECT_NAME} SHARED src/initial_pose_adaptor.cpp + src/routing_adaptor.cpp ) -ament_auto_add_executable(routing_adaptor - src/routing_adaptor.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ad_api_adaptors::InitialPoseAdaptor" + EXECUTABLE initial_pose_adaptor_node + EXECUTOR MultiThreadedExecutor +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ad_api_adaptors::RoutingAdaptor" + EXECUTABLE routing_adaptor_node + EXECUTOR SingleThreadedExecutor ) ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml index 4d00e254e78d5..855f57345ed15 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/launch/rviz_adaptors.launch.xml @@ -3,7 +3,7 @@ - + @@ -12,7 +12,7 @@ - + diff --git a/system/default_ad_api_helpers/ad_api_adaptors/package.xml b/system/default_ad_api_helpers/ad_api_adaptors/package.xml index 785ff58db1f81..b070131f1d567 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/package.xml +++ b/system/default_ad_api_helpers/ad_api_adaptors/package.xml @@ -17,6 +17,7 @@ component_interface_utils map_height_fitter rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp index 5d34653244ea2..f3c0ab9213656 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.cpp @@ -34,7 +34,8 @@ std::array get_covariance_parameter(rclcpp::Node * node, const std:: return array; } -InitialPoseAdaptor::InitialPoseAdaptor() : Node("initial_pose_adaptor"), fitter_(this) +InitialPoseAdaptor::InitialPoseAdaptor(const rclcpp::NodeOptions & options) +: Node("initial_pose_adaptor", options), fitter_(this) { rviz_particle_covariance_ = get_covariance_parameter(this, "initial_pose_particle_covariance"); sub_initial_pose_ = create_subscription( @@ -61,13 +62,5 @@ void InitialPoseAdaptor::on_initial_pose(const PoseWithCovarianceStamped::ConstS } // namespace ad_api_adaptors -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::InitialPoseAdaptor) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp index 0531afd55ac21..340bc3b0a3058 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/initial_pose_adaptor.hpp @@ -28,7 +28,7 @@ namespace ad_api_adaptors class InitialPoseAdaptor : public rclcpp::Node { public: - InitialPoseAdaptor(); + explicit InitialPoseAdaptor(const rclcpp::NodeOptions & options); private: using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp index 7151902a972d6..18421c976cf41 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.cpp @@ -19,7 +19,8 @@ namespace ad_api_adaptors { -RoutingAdaptor::RoutingAdaptor() : Node("routing_adaptor") +RoutingAdaptor::RoutingAdaptor(const rclcpp::NodeOptions & options) +: Node("routing_adaptor", options) { using std::placeholders::_1; @@ -110,13 +111,5 @@ void RoutingAdaptor::on_reroute(const PoseStamped::ConstSharedPtr pose) } // namespace ad_api_adaptors -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::SingleThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ad_api_adaptors::RoutingAdaptor) diff --git a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp index 4040ee37ead3e..877d705825733 100644 --- a/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp +++ b/system/default_ad_api_helpers/ad_api_adaptors/src/routing_adaptor.hpp @@ -29,7 +29,7 @@ namespace ad_api_adaptors class RoutingAdaptor : public rclcpp::Node { public: - RoutingAdaptor(); + explicit RoutingAdaptor(const rclcpp::NodeOptions & options); private: using PoseStamped = geometry_msgs::msg::PoseStamped; diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt index ad65f04356f4d..b777df8675bef 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt +++ b/system/default_ad_api_helpers/automatic_pose_initializer/CMakeLists.txt @@ -4,8 +4,14 @@ project(automatic_pose_initializer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(automatic_pose_initializer +ament_auto_add_library(${PROJECT_NAME} SHARED src/automatic_pose_initializer.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "automatic_pose_initializer::AutomaticPoseInitializer" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + ament_auto_package(INSTALL_TO_SHARE launch) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml index ac5e63e84092a..e9a94efd6be7b 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/launch/automatic_pose_initializer.launch.xml @@ -1,6 +1,6 @@ - + diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml index be192c3dade4b..7321de4b1811d 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/package.xml +++ b/system/default_ad_api_helpers/automatic_pose_initializer/package.xml @@ -16,6 +16,7 @@ autoware_adapi_v1_msgs component_interface_utils rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp index 6970d489ff340..8347b479b22c1 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.cpp @@ -19,7 +19,8 @@ namespace automatic_pose_initializer { -AutomaticPoseInitializer::AutomaticPoseInitializer() : Node("automatic_pose_initializer") +AutomaticPoseInitializer::AutomaticPoseInitializer(const rclcpp::NodeOptions & options) +: Node("automatic_pose_initializer", options) { const auto adaptor = component_interface_utils::NodeAdaptor(this); group_cli_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -48,13 +49,5 @@ void AutomaticPoseInitializer::on_timer() } // namespace automatic_pose_initializer -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); -} +#include +RCLCPP_COMPONENTS_REGISTER_NODE(automatic_pose_initializer::AutomaticPoseInitializer) diff --git a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp index 80fe78832026d..91d70dfca3761 100644 --- a/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp +++ b/system/default_ad_api_helpers/automatic_pose_initializer/src/automatic_pose_initializer.hpp @@ -25,7 +25,7 @@ namespace automatic_pose_initializer class AutomaticPoseInitializer : public rclcpp::Node { public: - AutomaticPoseInitializer(); + explicit AutomaticPoseInitializer(const rclcpp::NodeOptions & options); private: void on_timer(); diff --git a/system/emergency_handler/CMakeLists.txt b/system/emergency_handler/CMakeLists.txt index b7895b07f6e4c..0475cdbe57dd8 100644 --- a/system/emergency_handler/CMakeLists.txt +++ b/system/emergency_handler/CMakeLists.txt @@ -4,11 +4,16 @@ project(emergency_handler) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(emergency_handler - src/emergency_handler/emergency_handler_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/emergency_handler/emergency_handler_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "EmergencyHandler" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 5c76d96e573ab..a78b35be26daf 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -53,7 +53,7 @@ struct Param class EmergencyHandler : public rclcpp::Node { public: - EmergencyHandler(); + explicit EmergencyHandler(const rclcpp::NodeOptions & options); private: // Subscribers diff --git a/system/emergency_handler/launch/emergency_handler.launch.xml b/system/emergency_handler/launch/emergency_handler.launch.xml index 308eaf90dbb69..203c13bd94e0a 100644 --- a/system/emergency_handler/launch/emergency_handler.launch.xml +++ b/system/emergency_handler/launch/emergency_handler.launch.xml @@ -16,7 +16,7 @@ - + diff --git a/system/emergency_handler/package.xml b/system/emergency_handler/package.xml index 10282297017b1..97344f9b8c594 100644 --- a/system/emergency_handler/package.xml +++ b/system/emergency_handler/package.xml @@ -18,6 +18,7 @@ autoware_auto_vehicle_msgs nav_msgs rclcpp + rclcpp_components std_msgs std_srvs tier4_system_msgs diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index db3ad40249bfb..2770a14691d95 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -18,7 +18,8 @@ #include #include -EmergencyHandler::EmergencyHandler() : Node("emergency_handler") +EmergencyHandler::EmergencyHandler(const rclcpp::NodeOptions & options) +: Node("emergency_handler", options) { // Parameter param_.update_rate = declare_parameter("update_rate"); @@ -459,3 +460,6 @@ bool EmergencyHandler::isStopped() return false; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EmergencyHandler) diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp deleted file mode 100644 index 5b60117cc3ff4..0000000000000 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_node.cpp +++ /dev/null @@ -1,32 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "emergency_handler/emergency_handler_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::executors::MultiThreadedExecutor executor; - auto node = std::make_shared(); - executor.add_node(node); - executor.spin(); - executor.remove_node(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/mrm_handler/CMakeLists.txt b/system/mrm_handler/CMakeLists.txt index 7c2174f03a6f3..93e03e7f20ead 100644 --- a/system/mrm_handler/CMakeLists.txt +++ b/system/mrm_handler/CMakeLists.txt @@ -4,11 +4,16 @@ project(mrm_handler) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(mrm_handler - src/mrm_handler/mrm_handler_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/mrm_handler/mrm_handler_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "MrmHandler" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + ament_auto_package(INSTALL_TO_SHARE launch config diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 1dd0f6b081337..c7aaca96aae49 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -60,7 +60,7 @@ struct Param class MrmHandler : public rclcpp::Node { public: - MrmHandler(); + explicit MrmHandler(const rclcpp::NodeOptions & options); private: // type diff --git a/system/mrm_handler/launch/mrm_handler.launch.xml b/system/mrm_handler/launch/mrm_handler.launch.xml index 562134f5301e2..7e761157956df 100644 --- a/system/mrm_handler/launch/mrm_handler.launch.xml +++ b/system/mrm_handler/launch/mrm_handler.launch.xml @@ -18,7 +18,7 @@ - + diff --git a/system/mrm_handler/package.xml b/system/mrm_handler/package.xml index e62091f2984e6..c15bc98a6c190 100644 --- a/system/mrm_handler/package.xml +++ b/system/mrm_handler/package.xml @@ -18,6 +18,7 @@ autoware_auto_vehicle_msgs nav_msgs rclcpp + rclcpp_components std_msgs std_srvs tier4_system_msgs diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index e8e692f755e2d..5e709946c2f46 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -18,7 +18,7 @@ #include #include -MrmHandler::MrmHandler() : Node("mrm_handler") +MrmHandler::MrmHandler(const rclcpp::NodeOptions & options) : Node("mrm_handler", options) { // Parameter param_.update_rate = declare_parameter("update_rate", 10); @@ -597,3 +597,6 @@ bool MrmHandler::isArrivedAtGoal() return operation_mode_state_->mode == OperationModeState::STOP; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(MrmHandler) diff --git a/tools/reaction_analyzer/CMakeLists.txt b/tools/reaction_analyzer/CMakeLists.txt new file mode 100644 index 0000000000000..d94a5bd7a794e --- /dev/null +++ b/tools/reaction_analyzer/CMakeLists.txt @@ -0,0 +1,40 @@ +cmake_minimum_required(VERSION 3.14) +project(reaction_analyzer) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(PCL REQUIRED) +find_package(Eigen3 REQUIRED) + +ament_auto_add_library(reaction_analyzer SHARED + include/reaction_analyzer_node.hpp + src/reaction_analyzer_node.cpp + include/subscriber.hpp + src/subscriber.cpp + include/topic_publisher.hpp + src/topic_publisher.cpp + include/utils.hpp + src/utils.cpp +) + +target_include_directories(reaction_analyzer + SYSTEM PUBLIC + ${PCL_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(reaction_analyzer + ${PCL_LIBRARIES} +) + +rclcpp_components_register_node(reaction_analyzer + PLUGIN "reaction_analyzer::ReactionAnalyzerNode" + EXECUTABLE reaction_analyzer_exe +) + +ament_auto_package( + INSTALL_TO_SHARE + param + launch +) diff --git a/tools/reaction_analyzer/README.md b/tools/reaction_analyzer/README.md new file mode 100644 index 0000000000000..f5a22aaf20176 --- /dev/null +++ b/tools/reaction_analyzer/README.md @@ -0,0 +1,229 @@ +# Reaction Analyzer + +## Description + +The main purpose of the reaction analyzer package is to measure the reaction times of various nodes within a ROS-based +autonomous driving simulation environment by subscribing to pre-determined topics. This tool is particularly useful for +evaluating the performance of perception, planning, and control pipelines in response to dynamic changes in the +environment, such as sudden obstacles. To be able to measure both control outputs and perception outputs, it was +necessary to divide the node into two running_mode: `planning_control` and `perception_planning`. + +![ReactionAnalyzerDesign.png](media%2FReactionAnalyzerDesign.png) + +### Planning Control Mode + +In this mode, the reaction analyzer creates a dummy publisher for the PredictedObjects and PointCloud2 topics. In the +beginning of the test, it publishes the initial position of the ego vehicle and the goal position to set the test +environment. Then, it spawns a sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to +search reacted messages of the planning and control nodes in the pre-determined topics. When all the topics are reacted, +it calculates the reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes +with `spawn_cmd_time`, and it creates a csv file to store the results. + +### Perception Planning Mode + +In this mode, the reaction analyzer reads the rosbag files which are recorded from AWSIM, and it creates a topic +publisher for each topic inside the rosbag to replay the rosbag. It reads two rosbag files: `path_bag_without_object` +and `path_bag_with_object`. Firstly, it replays the `path_bag_without_object` to set the initial position of the ego +vehicle and the goal position. After `spawn_time_after_init` seconds , it replays the `path_bag_with_object` to spawn a +sudden obstacle in front of the ego vehicle. After the obstacle is spawned, it starts to search the reacted messages of +the perception and planning nodes in the pre-determined topics. When all the topics are reacted, it calculates the +reaction time of the nodes and statistics by comparing `reacted_times` of each of the nodes with `spawn_cmd_time`, and +it creates a csv file to store the results. + +#### Point Cloud Publisher Type + +To get better analyze for Perception & Sensing pipeline, the reaction analyzer can publish the point cloud messages in 3 +different ways: `async_header_sync_publish`, `sync_header_sync_publish` or `async_publish`. (`T` is the period of the +lidar's output) + +![PointcloudPublisherType.png](media%2FPointcloudPublisherType.png) + +- `async_header_sync_publish`: It publishes the point cloud messages synchronously with asynchronous header times. It + means that each of the lidar's output will be published at the same time, but the headers of the point cloud messages + includes different timestamps because of the phase difference. +- `sync_header_sync_publish`: It publishes the point cloud messages synchronously with synchronous header times. It + means that each of the lidar's output will be published at the same time, and the headers of the point cloud messages + includes the same timestamps. +- `async_publish`: It publishes the point cloud messages asynchronously. It means that each of the lidar's output will + be published at different times. + +## Usage + +The common parameters you need to define for both running modes are `output_file_path`, `test_iteration`, +and `reaction_chain` list. `output_file_path` is the output file path is the path where the results and statistics +will be stored. `test_iteration` defines how many tests will be performed. The `reaction_chain` list is the list of the +pre-defined topics you want to measure their reaction times. + +**IMPORTANT:** Ensure the `reaction_chain` list is correctly defined: + +- For `perception_planning` mode, **do not** define `Control` nodes. +- For `planning_control` mode, **do not** define `Perception` nodes. + +### Prepared Test Environment + +- Download the demonstration test map from the + link [here](https://github.com/tier4/AWSIM/releases/download/v1.1.0/nishishinjuku_autoware_map.zip). After + downloading, + extract the zip file and use its path as `[MAP_PATH]` in the following commands. + +#### Planning Control Mode + +- You need to define only Planning and Control nodes in the `reaction_chain` list. With the default parameters, + you can start to test with the following command: + +```bash +ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=planning_control vehicle_model:=sample_vehicle sensor_model:=sample_sensor_kit map_path:=[MAP_PATH] +``` + +After the command, the `simple_planning_simulator` and the `reaction_analyzer` will be launched. It will automatically +start to test. After the test is completed, the results will be stored in the `output_file_path` you defined. + +#### Perception Planning Mode + +- Download the rosbag files from the Google Drive + link [here](https://drive.google.com/file/d/1-Qcv7gYfR-usKOjUH8I997w8I4NMhXlX/view?usp=sharing). +- Extract the zip file and set the path of the `.db3` files to parameters `path_bag_without_object` + and `path_bag_with_object`. +- You can start to test with the following command: + +```bash +ros2 launch reaction_analyzer reaction_analyzer.launch.xml running_mode:=perception_planning vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH] +``` + +After the command, the `e2e_simulator` and the `reaction_analyzer` will be launched. It will automatically start +to test. After the test is completed, the results will be stored in the `output_file_path` you defined. + +#### Prepared Test Environment + +**Scene without object:** +![sc1-awsim.png](media%2Fsc1-awsim.png) +![sc1-rviz.png](media%2Fsc1-rviz.png) + +**Scene object:** +![sc2-awsim.png](media%2Fsc2-awsim.png) +![sc2-rviz.png](media%2Fsc2-rviz.png) + +### Custom Test Environment + +**If you want to run the reaction analyzer with your custom test environment, you need to redefine some of the +parameters. +The parameters you need to redefine are `initialization_pose`, `entity_params`, `goal_pose`, and `topic_publisher` ( +for `perception_planning` mode) parameters.** + +- To set `initialization_pose`, `entity_params`, `goal_pose`: +- Run the AWSIM environment. Tutorial for AWSIM can be found + [here](https://autowarefoundation.github.io/AWSIM/main/GettingStarted/QuickStartDemo/). +- Run the e2e_simulator with the following command: + +```bash +ros2 launch autoware_launch e2e_simulator.launch.xml vehicle_model:=sample_vehicle sensor_model:=awsim_labs_sensor_kit map_path:=[MAP_PATH] +``` + +- After EGO is initialized, you can move the ego vehicle to the desired position by using the `SetGoal` button in the + RViz. +- After the EGO stopped in desired position, please localize the dummy obstacle by using the traffic controller. You can + control the traffic by pressing `ESC` button. + +**After localize EGO and dummy vehicle, we should write the positions of these entities in the map frame +in `reaction_analyzer.param.yaml`. To achieve this:** + +- Get initialization pose from `/awsim/ground_truth/vehicle/pose` topic. +- Get entity params from `/perception/object_recognition/objects` topic. +- Get goal pose from `/planning/mission_planning/goal` topic. + +**PS: `initialization_pose` is only valid for `planning_control` mode.** + +- After the parameters were noted, we should record the rosbags for the test. To record the rosbags, you can use the + following command: + +```bash +ros2 bag record --all +``` + +- You should record two rosbags: one without the object and one with the object. You can use the traffic controller to + spawn the object in front of the EGO vehicle or remove it. + +**NOTE: You should record the rosbags in the same environment with the same position of the EGO vehicle. You don't need +to run Autoware while recording.** + +- After you record the rosbags, you can set the `path_bag_without_object` and `path_bag_with_object` parameters with the + paths of the recorded rosbags. + +## Results + +The results will be stored in the `csv` file format and written to the `output_file_path` you defined. It shows each +pipeline of the Autoware by using header timestamp of the messages, and it reports `Node Latency`, `Pipeline Latency`, +and `Total Latency` +for each of the nodes. + +- `Node Latency`: The time difference between previous and current node's reaction timestamps. If it is the first node + in the pipeline, it is same as `Pipeline Latency`. +- `Pipeline Latency`: The time difference between published time of the message and pipeline header time. +- `Total Latency`: The time difference between the message's published timestamp and the spawn obstacle command sent + timestamp. + +## Parameters + +| Name | Type | Description | +| ---------------------------------------------------------------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------------------------- | +| `timer_period` | double | [s] Period for the main processing timer. | +| `test_iteration` | int | Number of iterations for the test. | +| `output_file_path` | string | Directory path where test results and statistics will be stored. | +| `spawn_time_after_init` | double | [s] Time delay after initialization before spawning objects. Only valid `perception_planning` mode. | +| `spawn_distance_threshold` | double | [m] Distance threshold for spawning objects. Only valid `planning_control` mode. | +| `poses.initialization_pose` | struct | Initial pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. Only valid `planning_control` mode. | +| `poses.entity_params` | struct | Parameters for entities (e.g., obstacles), containing `x`, `y`, `z`, `roll`, `pitch`, `yaw`, `x_dimension`, `y_dimension`, and `z_dimension`. | +| `poses.goal_pose` | struct | Goal pose of the vehicle, containing `x`, `y`, `z`, `roll`, `pitch`, and `yaw` fields. | +| `topic_publisher.path_bag_without_object` | string | Path to the ROS bag file without objects. Only valid `perception_planning` mode. | +| `topic_publisher.path_bag_with_object` | string | Path to the ROS bag file with objects. Only valid `perception_planning` mode. | +| `topic_publisher.spawned_pointcloud_sampling_distance` | double | [m] Sampling distance for point clouds of spawned objects. Only valid `planning_control` mode. | +| `topic_publisher.dummy_perception_publisher_period` | double | [s] Publishing period for the dummy perception data. Only valid `planning_control` mode. | +| `topic_publisher.pointcloud_publisher.pointcloud_publisher_type` | string | Defines how the PointCloud2 messages are going to be published. Modes explained above. | +| `topic_publisher.pointcloud_publisher.pointcloud_publisher_period` | double | [s] Publishing period of the PointCloud2 messages. | +| `topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object` | bool | Default false. Publish only the point cloud messages with the object. | +| `reaction_params.first_brake_params.debug_control_commands` | bool | Debug publish flag. | +| `reaction_params.first_brake_params.control_cmd_buffer_time_interval` | double | [s] Time interval for buffering control commands. | +| `reaction_params.first_brake_params.min_number_descending_order_control_cmd` | int | Minimum number of control commands in descending order for triggering brake. | +| `reaction_params.first_brake_params.min_jerk_for_brake_cmd` | double | [m/s³] Minimum jerk value for issuing a brake command. | +| `reaction_params.search_zero_vel_params.max_looking_distance` | double | [m] Maximum looking distance for zero velocity on trajectory | +| `reaction_params.search_entity_params.search_radius` | double | [m] Searching radius for spawned entity. Distance between ego pose and entity pose. | +| `reaction_chain` | struct | List of the nodes with their topics and topic's message types. | + +## Limitations + +- Reaction analyzer has some limitation like `PublisherMessageType`, `SubscriberMessageType` and `ReactionType`. It is + currently supporting following list: + +- **Publisher Message Types:** + + - `sensor_msgs/msg/PointCloud2` + - `sensor_msgs/msg/CameraInfo` + - `sensor_msgs/msg/Image` + - `geometry_msgs/msg/PoseWithCovarianceStamped` + - `sensor_msgs/msg/Imu` + - `autoware_auto_vehicle_msgs/msg/ControlModeReport` + - `autoware_auto_vehicle_msgs/msg/GearReport` + - `autoware_auto_vehicle_msgs/msg/HazardLightsReport` + - `autoware_auto_vehicle_msgs/msg/SteeringReport` + - `autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport` + - `autoware_auto_vehicle_msgs/msg/VelocityReport` + +- **Subscriber Message Types:** + + - `sensor_msgs/msg/PointCloud2` + - `autoware_auto_perception_msgs/msg/DetectedObjects` + - `autoware_auto_perception_msgs/msg/TrackedObjects` + - `autoware_auto_msgs/msg/PredictedObject` + - `autoware_auto_planning_msgs/msg/Trajectory` + - `autoware_auto_control_msgs/msg/AckermannControlCommand` + +- **Reaction Types:** + - `FIRST_BRAKE` + - `SEARCH_ZERO_VEL` + - `SEARCH_ENTITY` + +## Future improvements + +- The reaction analyzer can be improved by adding more reaction types. Currently, it is supporting only `FIRST_BRAKE`, + `SEARCH_ZERO_VEL`, and `SEARCH_ENTITY` reaction types. It can be extended by adding more reaction types for each of + the message types. diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp new file mode 100644 index 0000000000000..6dc3dd896dc92 --- /dev/null +++ b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp @@ -0,0 +1,158 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef REACTION_ANALYZER_NODE_HPP_ +#define REACTION_ANALYZER_NODE_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer +{ +using autoware_adapi_v1_msgs::msg::LocalizationInitializationState; +using autoware_adapi_v1_msgs::msg::OperationModeState; +using autoware_adapi_v1_msgs::msg::RouteState; +using autoware_adapi_v1_msgs::srv::ChangeOperationMode; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_internal_msgs::msg::PublishedTime; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +struct NodeParams +{ + std::string running_mode; + double timer_period; + std::string output_file_path; + size_t test_iteration; + double spawn_time_after_init; + double spawn_distance_threshold; + PoseParams initial_pose; + PoseParams goal_pose; + EntityParams entity_params; +}; + +class ReactionAnalyzerNode : public rclcpp::Node +{ +public: + explicit ReactionAnalyzerNode(rclcpp::NodeOptions node_options); + + Odometry::ConstSharedPtr odometry_ptr_; + +private: + std::mutex mutex_; + RunningMode node_running_mode_; + + // Parameters + NodeParams node_params_; + + // Initialization Variables + geometry_msgs::msg::Pose entity_pose_; + geometry_msgs::msg::PoseWithCovarianceStamped init_pose_; + geometry_msgs::msg::PoseStamped goal_pose_; + + // Subscribers + rclcpp::Subscription::SharedPtr sub_kinematics_; + rclcpp::Subscription::SharedPtr sub_route_state_; + rclcpp::Subscription::SharedPtr sub_localization_init_state_; + rclcpp::Subscription::SharedPtr sub_operation_mode_; + rclcpp::Subscription::SharedPtr + sub_ground_truth_pose_; // Only for perception_planning mode + + // Publishers + rclcpp::Publisher::SharedPtr pub_initial_pose_; + rclcpp::Publisher::SharedPtr pub_goal_pose_; + rclcpp::Publisher::SharedPtr pub_marker_; + + // Variables + std::vector pipeline_map_vector_; + std::optional last_test_environment_init_request_time_; + std::optional test_environment_init_time_; + std::optional spawn_cmd_time_; + std::atomic spawn_object_cmd_{false}; + std::atomic ego_initialized_{false}; + bool is_initialization_requested{false}; + bool is_route_set_{false}; + size_t test_iteration_count_{0}; + visualization_msgs::msg::Marker entity_debug_marker_; + + // Functions + void init_analyzer_variables(); + void init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + bool init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time); + bool set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time); + bool check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr); + void call_operation_mode_service_without_response(); + void spawn_obstacle(const geometry_msgs::msg::Point & ego_pose); + void calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time); + void on_timer(); + void reset(); + + // Callbacks + void on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr); + void on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr); + void on_route_state(RouteState::ConstSharedPtr msg_ptr); + void on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr); + void on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + // Client + rclcpp::Client::SharedPtr client_change_to_autonomous_; + + // Pointers + std::unique_ptr topic_publisher_ptr_; + std::unique_ptr subscriber_ptr_; + LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_; + RouteState::ConstSharedPtr current_route_state_ptr_; + OperationModeState::ConstSharedPtr operation_mode_ptr_; + PoseStamped::ConstSharedPtr ground_truth_pose_ptr_; +}; +} // namespace reaction_analyzer + +#endif // REACTION_ANALYZER_NODE_HPP_ diff --git a/tools/reaction_analyzer/include/subscriber.hpp b/tools/reaction_analyzer/include/subscriber.hpp new file mode 100644 index 0000000000000..d14d41da545f7 --- /dev/null +++ b/tools/reaction_analyzer/include/subscriber.hpp @@ -0,0 +1,191 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SUBSCRIBER_HPP_ +#define SUBSCRIBER_HPP_ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_internal_msgs::msg::PublishedTime; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +// Buffers to be used to store subscribed messages' data, pipeline Header, and PublishedTime +using MessageBuffer = std::optional; +// We need to store the past AckermannControlCommands to analyze the first brake +using ControlCommandBuffer = std::pair, MessageBuffer>; +// Variant to store different types of buffers +using MessageBufferVariant = std::variant; + +template +struct SubscriberVariables +{ + using ExactTimePolicy = message_filters::sync_policies::ExactTime; + + std::unique_ptr> sub1_; + std::unique_ptr> sub2_; + std::unique_ptr> synchronizer_; + // tmp: only for the messages who don't have header e.g. AckermannControlCommand + std::unique_ptr> cache_; +}; + +// Variant to create subscribers for different message types +using SubscriberVariablesVariant = std::variant< + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables, + SubscriberVariables, SubscriberVariables>; + +// The configuration of the topic to be subscribed which are defined in reaction_chain +struct TopicConfig +{ + std::string node_name; + std::string topic_address; + std::string time_debug_topic_address; + SubscriberMessageType message_type; +}; + +// Place for the reaction functions' parameter configuration +struct FirstBrakeParams +{ + bool debug_control_commands; + double control_cmd_buffer_time_interval; + size_t min_number_descending_order_control_cmd; + double min_jerk_for_brake_cmd; +}; + +struct SearchZeroVelParams +{ + double max_looking_distance; +}; + +struct SearchEntityParams +{ + double search_radius_offset; +}; + +// Place for the store the reaction parameter configuration (currently only for first brake) +struct ReactionParams +{ + FirstBrakeParams first_brake_params; + SearchZeroVelParams search_zero_vel_params; + SearchEntityParams search_entity_params; +}; + +using ChainModules = std::vector; + +class SubscriberBase +{ +public: + explicit SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params); + + std::optional> get_message_buffers_map(); + void reset(); + +private: + std::mutex mutex_; + + // Init + rclcpp::Node * node_; + Odometry::ConstSharedPtr odometry_; + std::atomic & spawn_object_cmd_; + EntityParams entity_params_; + + // Variables to be initialized in constructor + ChainModules chain_modules_{}; + ReactionParams reaction_params_{}; + geometry_msgs::msg::Pose entity_pose_{}; + double entity_search_radius_{0.0}; + + // Variants + std::map subscriber_variables_map_; + std::map message_buffers_; + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Functions + void init_reaction_chains_and_params(); + bool init_subscribers(); + std::optional get_subscriber_variable( + const TopicConfig & topic_config); + std::optional find_first_brake_idx( + const std::vector & cmd_array); + void set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const; + + // Callbacks for modules are subscribed + void on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr); + void on_trajectory(const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr); + void on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_pointcloud(const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr); + void on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr); + void on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr); + void on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr); + void on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr); +}; + +} // namespace reaction_analyzer::subscriber + +#endif // SUBSCRIBER_HPP_ diff --git a/tools/reaction_analyzer/include/topic_publisher.hpp b/tools/reaction_analyzer/include/topic_publisher.hpp new file mode 100644 index 0000000000000..c6d3a90650884 --- /dev/null +++ b/tools/reaction_analyzer/include/topic_publisher.hpp @@ -0,0 +1,249 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef TOPIC_PUBLISHER_HPP_ +#define TOPIC_PUBLISHER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace reaction_analyzer::topic_publisher +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using geometry_msgs::msg::Pose; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; + +struct TopicPublisherParams +{ + double dummy_perception_publisher_period; // Only for planning_control mode + double spawned_pointcloud_sampling_distance; + std::string path_bag_without_object; // Path to the bag file without object + std::string path_bag_with_object; // Path to the bag file with object + std::string pointcloud_publisher_type; // Type of the pointcloud publisher + double pointcloud_publisher_period; // Period of the pointcloud publisher + bool publish_only_pointcloud_with_object; // Publish only pointcloud with object for only + // perception pipeline debug purpose make it true. +}; + +enum class PointcloudPublisherType { + ASYNC_PUBLISHER = 0, // Asynchronous publisher + SYNC_HEADER_SYNC_PUBLISHER = 1, // Synchronous publisher with header synchronization + ASYNC_HEADER_SYNC_PUBLISHER = 2, // Asynchronous publisher with header synchronization +}; + +/** + * @brief Message type template struct for the variables of the Publisher. + */ +template +struct PublisherVariables +{ + std::chrono::milliseconds period_ms{0}; + typename MessageType::SharedPtr empty_area_message; + typename MessageType::SharedPtr object_spawned_message; + typename rclcpp::Publisher::SharedPtr publisher; + rclcpp::TimerBase::SharedPtr timer; +}; + +/** + * @brief Struct for accessing the variables of the Publisher. + */ +struct PublisherVarAccessor +{ + // Template struct to check if a type has a header member. + template > + struct HasHeader : std::false_type + { + }; + + template + struct HasHeader> : std::true_type + { + }; + + // Template struct to check if a type has a stamp member. + template > + struct HasStamp : std::false_type + { + }; + + template + struct HasStamp> : std::true_type + { + }; + + template + void publish_with_current_time( + const PublisherVariables & publisher_var, const rclcpp::Time & current_time, + const bool is_object_spawned) const + { + std::unique_ptr msg_to_be_published = std::make_unique(); + + if (is_object_spawned) { + *msg_to_be_published = *publisher_var.object_spawned_message; + } else { + *msg_to_be_published = *publisher_var.empty_area_message; + } + if constexpr (HasHeader::value) { + msg_to_be_published->header.stamp = current_time; + } else if constexpr (HasStamp::value) { + msg_to_be_published->stamp = current_time; + } + publisher_var.publisher->publish(std::move(msg_to_be_published)); + } + + template + void set_period(T & publisher_var, std::chrono::milliseconds new_period) + { + publisher_var.period_ms = new_period; + } + + template + std::chrono::milliseconds get_period(const T & publisher_var) const + { + return publisher_var.period_ms; + } + + template + std::shared_ptr get_empty_area_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.empty_area_message); + } + + template + std::shared_ptr get_object_spawned_message(const T & publisher_var) const + { + return std::static_pointer_cast(publisher_var.object_spawned_message); + } +}; + +using PublisherVariablesVariant = std::variant< + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables, + PublisherVariables>; + +using LidarOutputPair = std::pair< + std::shared_ptr>, + std::shared_ptr>>; + +class TopicPublisher +{ +public: + explicit TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params); + + void reset(); + +private: + std::mutex mutex_; + + // Initialized variables + rclcpp::Node * node_; + RunningMode node_running_mode_; + std::atomic & spawn_object_cmd_; + std::atomic & ego_initialized_; // used for planning_control mode because if pointcloud is + // published before ego is initialized, it causes crash + EntityParams entity_params_; + std::optional & spawn_cmd_time_; // Set by a publisher function when the + // spawn_object_cmd_ is true + + // tf + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + + // Parameters + TopicPublisherParams topic_publisher_params_; + + // Variables planning_control mode + rclcpp::Publisher::SharedPtr pub_pointcloud_; + rclcpp::Publisher::SharedPtr pub_predicted_objects_; + PointCloud2::SharedPtr entity_pointcloud_ptr_; + PredictedObjects::SharedPtr predicted_objects_ptr_; + + // Variables perception_planning mode + PointcloudPublisherType pointcloud_publisher_type_; + std::map topic_publisher_map_; + std::map + lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex + bool is_object_spawned_message_published_{false}; + std::shared_ptr one_shot_timer_shared_ptr_; + + // Functions + template + void set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message); + void set_period(const std::map> & time_map); + void set_publishers_and_timers_to_variable(); + void set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map); + bool check_publishers_initialized_correctly(); + void init_rosbag_publishers(); + void init_rosbag_publisher_buffer(const std::string & bag_path, const bool is_empty_area_message); + void pointcloud_messages_sync_publisher(const PointcloudPublisherType type); + void pointcloud_messages_async_publisher( + const std::pair< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_); + void generic_message_publisher(const std::string & topic_name); + void dummy_perception_publisher(); // Only for planning_control mode + + // Timers + std::map pointcloud_publish_timers_map_; + rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_; + rclcpp::TimerBase::SharedPtr dummy_perception_timer_; +}; +} // namespace reaction_analyzer::topic_publisher + +#endif // TOPIC_PUBLISHER_HPP_ diff --git a/tools/reaction_analyzer/include/utils.hpp b/tools/reaction_analyzer/include/utils.hpp new file mode 100644 index 0000000000000..da1defc03f34c --- /dev/null +++ b/tools/reaction_analyzer/include/utils.hpp @@ -0,0 +1,363 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +// The reaction_analyzer namespace contains utility functions for the Reaction Analyzer tool. +namespace reaction_analyzer +{ +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_perception_msgs::msg::DetectedObject; +using autoware_auto_perception_msgs::msg::DetectedObjects; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_internal_msgs::msg::PublishedTime; +using sensor_msgs::msg::PointCloud2; + +/** + * @brief A pair containing the ReactionPair. + * The first element is name of the node that published the PublishedTime msg. + * The second element is the PublishedTime msg itself + */ +using ReactionPair = std::pair; + +/** + * @brief A map containing the pipeline and the reaction pairs. + * The key is the time at which the pipeline was executed. + * The value is a vector of ReactionPair. + */ +using PipelineMap = std::map>; + +/** + * @brief A vector containing the pipeline and the reaction pairs. + * The first element is the time at which the pipeline was executed. + * The second element is a vector of ReactionPair. + */ +using TimestampedReactionPairsVector = + std::vector>>; + +/** + * @brief Enum for the different types of messages that can be published. + */ +enum class PublisherMessageType { + UNKNOWN = 0, + CAMERA_INFO = 1, + IMAGE = 2, + POINTCLOUD2 = 3, + POSE_WITH_COVARIANCE_STAMPED = 4, + POSE_STAMPED = 5, + ODOMETRY = 6, + IMU = 7, + CONTROL_MODE_REPORT = 8, + GEAR_REPORT = 9, + HAZARD_LIGHTS_REPORT = 10, + STEERING_REPORT = 11, + TURN_INDICATORS_REPORT = 12, + VELOCITY_REPORT = 13, +}; + +/** + * @brief Enum for the different types of messages that can be subscribed to. + */ +enum class SubscriberMessageType { + UNKNOWN = 0, + ACKERMANN_CONTROL_COMMAND = 1, + TRAJECTORY = 2, + POINTCLOUD2 = 3, + DETECTED_OBJECTS = 4, + PREDICTED_OBJECTS = 5, + TRACKED_OBJECTS = 6, +}; + +/** + * @brief Enum for the different types of reactions that can be analyzed. + */ +enum class ReactionType { + UNKNOWN = 0, + FIRST_BRAKE = 1, + SEARCH_ZERO_VEL = 2, + SEARCH_ENTITY = 3, +}; + +/** + * @brief Enum for the different running modes of the Reaction Analyzer. + */ +enum class RunningMode { + PerceptionPlanning = 0, + PlanningControl = 1, +}; + +/** + * @brief Structs containing the parameters of a pose. + */ +struct PoseParams +{ + double x; + double y; + double z; + double roll; + double pitch; + double yaw; +}; + +struct EntityParams +{ + double x; + double y; + double z; + double roll; + double pitch; + double yaw; + double x_l; + double y_l; + double z_l; +}; + +/** + * @brief Struct containing statistics of the latencies. + */ +struct LatencyStats +{ + double min; + double max; + double mean; + double median; + double std_dev; +}; + +/** + * @brief Convert string to SubscriberMessageType. + */ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type); + +/** + * @brief Convert string to PublisherMessageType. + */ +PublisherMessageType get_publisher_message_type(const std::string & message_type); + +/** + * @brief Get the PublisherMessageType for a given topic. + */ +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name); + +/** + * @brief Convert string to ReactionType. + */ +ReactionType get_reaction_type(const std::string & reaction_type); + +/** + * @brief Calculates the statistics of a vector of latencies. + * @param latency_vec The vector of latencies. + * @return The statistics of the latencies. + */ +LatencyStats calculate_statistics(const std::vector & latency_vec); + +/** + * @brief Generates a UUID message from a string. + * @param input The string to generate the UUID from. + * @return The generated UUID message. + */ +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input); + +/** + * @brief Generate pose from PoseParams. + * @param pose_params + * @return Pose + */ +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params); + +/** + * @brief Generate entity pose from entity params. + * @param entity_params + * @return Pose + */ +double calculate_entity_search_radius(const EntityParams & entity_params); + +/** + * @brief Create a pointcloud from entity params. + * @param entity_params + * @param pointcloud_sampling_distance + * @return PointCloud2::SharedPtr + */ +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance); + +/** + * @brief Create a predicted objects message from entity params. + * @param entity_params + * @return PredictedObjects::SharedPtr + */ +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params); + +/** + * @brief Creates a subscription option with a new callback group. + * + * @param node A pointer to the node for which the subscription options are being created. + * @return The created SubscriptionOptions. + */ +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node); + +/** + * @brief Creates a visualization marker for a polyhedron based on the provided entity parameters. + * + * @param params The parameters of the entity for which the marker is to be created. It includes the + * position, orientation, and dimensions of the entity. + * @return The created visualization marker for the polyhedron. + */ +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params); + +/** + * @brief Splits a string by a given delimiter. + * + * @param str The string to be split. + * @param delimiter The delimiter to split the string by. + * @return A vector of strings, each of which is a segment of the original string split by the + * delimiter. + */ +std::vector split(const std::string & str, char delimiter); + +/** + * @brief Checks if a folder exists. + * + * @param path The path to the folder. + * @return True if the folder exists, false otherwise. + */ +bool does_folder_exist(const std::string & path); + +/** + * @brief Get the index of the trajectory point that is a certain distance away from the current + * point. + * + * @param traj The trajectory to search. + * @param curr_id The index of the current point in the trajectory. + * @param distance The distance to search for a point. + * @return The index of the point that is at least the specified distance away from the current + * point. + */ +size_t get_index_after_distance( + const Trajectory & traj, const size_t curr_id, const double distance); + +/** + * @brief Search for a pointcloud near the pose. + * @param pcl_pointcloud, pose, search_radius + * @return bool + */ +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * + * @brief Search for a predicted object near the pose. + * @param predicted_objects, pose, search_radius + * @return bool + */ +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a detected object near the pose. + * @param detected_objects, pose, search_radius + * @return bool + */ +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); +/** + * @brief Search for a tracked object near the pose. + * @param tracked_objects, pose, search_radius + * @return bool + */ +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius); + +/** + * Calculates the time difference in milliseconds between two rclcpp::Time instances. + * + * @param start The start time. + * @param end The end time. + * @return The time difference in milliseconds as a double. + */ +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end); + +/** + * @brief Converts a PipelineMap to a PipelineMapVector. + * + * @param pipelineMap The PipelineMap to be converted. + * @return The PipelineMapVector that is equivalent to the PipelineMap. + */ +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap); + +/** + * @brief Writes the results to a file. + * + * @param node A pointer to the node for which the results are being written. + * @param output_file_path The path to the file where the results will be written. + * @param node_running_mode The running mode of the node. + * @param pipeline_map_vector The vector of PipelineMap containing the results to be written. + */ +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector); +} // namespace reaction_analyzer + +#endif // UTILS_HPP_ diff --git a/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml new file mode 100644 index 0000000000000..b0b1d4fb9bf2e --- /dev/null +++ b/tools/reaction_analyzer/launch/reaction_analyzer.launch.xml @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/reaction_analyzer/media/PointcloudPublisherType.png b/tools/reaction_analyzer/media/PointcloudPublisherType.png new file mode 100644 index 0000000000000..597b73318bcac Binary files /dev/null and b/tools/reaction_analyzer/media/PointcloudPublisherType.png differ diff --git a/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png new file mode 100644 index 0000000000000..cb0f2dd8577fc Binary files /dev/null and b/tools/reaction_analyzer/media/ReactionAnalyzerDesign.png differ diff --git a/tools/reaction_analyzer/media/sc1-awsim.png b/tools/reaction_analyzer/media/sc1-awsim.png new file mode 100644 index 0000000000000..6bdc104a93b87 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc1-rviz.png b/tools/reaction_analyzer/media/sc1-rviz.png new file mode 100644 index 0000000000000..ae7c6e882bf98 Binary files /dev/null and b/tools/reaction_analyzer/media/sc1-rviz.png differ diff --git a/tools/reaction_analyzer/media/sc2-awsim.png b/tools/reaction_analyzer/media/sc2-awsim.png new file mode 100644 index 0000000000000..8224b516897be Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-awsim.png differ diff --git a/tools/reaction_analyzer/media/sc2-rviz.png b/tools/reaction_analyzer/media/sc2-rviz.png new file mode 100644 index 0000000000000..8f4f5feae8ba3 Binary files /dev/null and b/tools/reaction_analyzer/media/sc2-rviz.png differ diff --git a/tools/reaction_analyzer/package.xml b/tools/reaction_analyzer/package.xml new file mode 100644 index 0000000000000..05081eac01751 --- /dev/null +++ b/tools/reaction_analyzer/package.xml @@ -0,0 +1,49 @@ + + + + reaction_analyzer + 1.0.0 + Analyzer that measures reaction times of the nodes + + Berkay Karaman + + Apache License 2.0 + + Berkay Karaman + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_control_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + autoware_internal_msgs + eigen + libpcl-all-dev + message_filters + motion_utils + pcl_conversions + pcl_ros + rclcpp + rclcpp_components + rosbag2_cpp + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + + ament_cmake_ros + ament_index_python + ament_lint_auto + autoware_lint_common + autoware_testing + + + ament_cmake + + diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml new file mode 100644 index 0000000000000..62c1191bd345a --- /dev/null +++ b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml @@ -0,0 +1,117 @@ +/**: + ros__parameters: + timer_period: 0.033 # s + test_iteration: 10 + output_file_path: + spawn_time_after_init: 10.0 # s for perception_planning mode + spawn_distance_threshold: 15.0 # m # for planning_control mode + poses: + initialization_pose: + x: 81546.984375 + y: 50011.96875 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 11.1130405 + goal_pose: + x: 81643.0703125 + y: 50029.8828125 + z: 0.0 + roll: 0.0 + pitch: 0.0 + yaw: 13.12 + entity_params: + x: 81633.86068376624 + y: 50028.383586673124 + z: 42.44818343779461 + roll: 0.0 + pitch: 0.0 + yaw: 11.8235848 + x_dimension: 3.95 + y_dimension: 1.77 + z_dimension: 1.43 + topic_publisher: + path_bag_without_object: /rosbag2_awsim_labs/rosbag2_awsim_labs.db3 + path_bag_with_object: /rosbag2_awsim_labs_obstacle/rosbag2_awsim_labs_obstacle.db3 + pointcloud_publisher: + pointcloud_publisher_type: "sync_header_sync_publish" # "async_header_sync_publish", "sync_header_sync_publish" or "async_publish" + pointcloud_publisher_period: 0.1 # s + publish_only_pointcloud_with_object: false # use it only for debug purposes. If true, only pointclouds with object will be published + spawned_pointcloud_sampling_distance: 0.1 # m for planning_control mode + dummy_perception_publisher_period: 0.1 # s for planning_control mode + subscriber: + reaction_params: + first_brake_params: + debug_control_commands: false + control_cmd_buffer_time_interval: 1.0 # s + min_number_descending_order_control_cmd: 3 + min_jerk_for_brake_cmd: 0.3 # m/s^3 + search_zero_vel_params: + max_looking_distance: 15.0 # m + search_entity_params: + search_radius_offset: 0.0 # m + reaction_chain: + obstacle_cruise_planner: + topic_name: /planning/scenario_planning/lane_driving/trajectory + time_debug_topic_name: /planning/scenario_planning/lane_driving/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + scenario_selector: + topic_name: /planning/scenario_planning/scenario_selector/trajectory + time_debug_topic_name: /planning/scenario_planning/scenario_selector/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + motion_velocity_smoother: + topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory + time_debug_topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + planning_validator: + topic_name: /planning/scenario_planning/trajectory + time_debug_topic_name: /planning/scenario_planning/trajectory/debug/published_time + message_type: autoware_auto_planning_msgs/msg/Trajectory + trajectory_follower: + topic_name: /control/trajectory_follower/control_cmd + time_debug_topic_name: /control/trajectory_follower/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + vehicle_cmd_gate: + topic_name: /control/command/control_cmd + time_debug_topic_name: /control/command/control_cmd/debug/published_time + message_type: autoware_auto_control_msgs/msg/AckermannControlCommand + common_ground_filter: + topic_name: /perception/obstacle_segmentation/single_frame/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/single_frame/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + occupancy_grid_map_outlier: + topic_name: /perception/obstacle_segmentation/pointcloud + time_debug_topic_name: /perception/obstacle_segmentation/pointcloud/debug/published_time + message_type: sensor_msgs/msg/PointCloud2 + multi_object_tracker: + topic_name: /perception/object_recognition/tracking/near_objects + time_debug_topic_name: /perception/object_recognition/tracking/near_objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + lidar_centerpoint: + topic_name: /perception/object_recognition/detection/centerpoint/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + obstacle_pointcloud_based_validator: + topic_name: /perception/object_recognition/detection/centerpoint/validation/objects + time_debug_topic_name: /perception/object_recognition/detection/centerpoint/validation/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + decorative_tracker_merger: + topic_name: /perception/object_recognition/tracking/objects + time_debug_topic_name: /perception/object_recognition/tracking/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/TrackedObjects + detected_object_feature_remover: + topic_name: /perception/object_recognition/detection/clustering/objects + time_debug_topic_name: /perception/object_recognition/detection/clustering/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + detection_by_tracker: + topic_name: /perception/object_recognition/detection/detection_by_tracker/objects + time_debug_topic_name: /perception/object_recognition/detection/detection_by_tracker/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + object_lanelet_filter: + topic_name: /perception/object_recognition/detection/objects + time_debug_topic_name: /perception/object_recognition/detection/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/DetectedObjects + map_based_prediction: + topic_name: /perception/object_recognition/objects + time_debug_topic_name: /perception/object_recognition/objects/debug/published_time + message_type: autoware_auto_perception_msgs/msg/PredictedObjects diff --git a/tools/reaction_analyzer/src/reaction_analyzer_node.cpp b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp new file mode 100644 index 0000000000000..53313f28eb4d9 --- /dev/null +++ b/tools/reaction_analyzer/src/reaction_analyzer_node.cpp @@ -0,0 +1,442 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "reaction_analyzer_node.hpp" + +#include +#include + +namespace reaction_analyzer +{ + +void ReactionAnalyzerNode::on_operation_mode_state(OperationModeState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + operation_mode_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_route_state(RouteState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + current_route_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_vehicle_pose(Odometry::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + odometry_ptr_ = msg_ptr; +} + +void ReactionAnalyzerNode::on_localization_initialization_state( + LocalizationInitializationState::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + initialization_state_ptr_ = std::move(msg_ptr); +} + +void ReactionAnalyzerNode::on_ground_truth_pose(PoseStamped::ConstSharedPtr msg_ptr) +{ + std::lock_guard lock(mutex_); + ground_truth_pose_ptr_ = std::move(msg_ptr); +} + +ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions node_options) +: Node("reaction_analyzer_node", node_options.automatically_declare_parameters_from_overrides(true)) +{ + using std::placeholders::_1; + + node_params_.running_mode = get_parameter("running_mode").as_string(); + + // set running mode + if (node_params_.running_mode == "planning_control") { + node_running_mode_ = RunningMode::PlanningControl; + } else if (node_params_.running_mode == "perception_planning") { + node_running_mode_ = RunningMode::PerceptionPlanning; + } else { + RCLCPP_ERROR(get_logger(), "Invalid running mode. Node couldn't be initialized. Failed."); + return; + } + + node_params_.output_file_path = get_parameter("output_file_path").as_string(); + // Check if the output file path is valid + if (!does_folder_exist(node_params_.output_file_path)) { + RCLCPP_ERROR(get_logger(), "Output file path is not valid. Node couldn't be initialized."); + return; + } + + node_params_.timer_period = get_parameter("timer_period").as_double(); + node_params_.test_iteration = get_parameter("test_iteration").as_int(); + node_params_.spawn_time_after_init = get_parameter("spawn_time_after_init").as_double(); + node_params_.spawn_distance_threshold = get_parameter("spawn_distance_threshold").as_double(); + + // Position parameters + node_params_.initial_pose.x = get_parameter("poses.initialization_pose.x").as_double(); + node_params_.initial_pose.y = get_parameter("poses.initialization_pose.y").as_double(); + node_params_.initial_pose.z = get_parameter("poses.initialization_pose.z").as_double(); + node_params_.initial_pose.roll = get_parameter("poses.initialization_pose.roll").as_double(); + node_params_.initial_pose.pitch = get_parameter("poses.initialization_pose.pitch").as_double(); + node_params_.initial_pose.yaw = get_parameter("poses.initialization_pose.yaw").as_double(); + + node_params_.goal_pose.x = get_parameter("poses.goal_pose.x").as_double(); + node_params_.goal_pose.y = get_parameter("poses.goal_pose.y").as_double(); + node_params_.goal_pose.z = get_parameter("poses.goal_pose.z").as_double(); + node_params_.goal_pose.roll = get_parameter("poses.goal_pose.roll").as_double(); + node_params_.goal_pose.pitch = get_parameter("poses.goal_pose.pitch").as_double(); + node_params_.goal_pose.yaw = get_parameter("poses.goal_pose.yaw").as_double(); + + node_params_.entity_params.x = get_parameter("poses.entity_params.x").as_double(); + node_params_.entity_params.y = get_parameter("poses.entity_params.y").as_double(); + node_params_.entity_params.z = get_parameter("poses.entity_params.z").as_double(); + node_params_.entity_params.roll = get_parameter("poses.entity_params.roll").as_double(); + node_params_.entity_params.pitch = get_parameter("poses.entity_params.pitch").as_double(); + node_params_.entity_params.yaw = get_parameter("poses.entity_params.yaw").as_double(); + node_params_.entity_params.x_l = get_parameter("poses.entity_params.x_dimension").as_double(); + node_params_.entity_params.y_l = get_parameter("poses.entity_params.y_dimension").as_double(); + node_params_.entity_params.z_l = get_parameter("poses.entity_params.z_dimension").as_double(); + + sub_kinematics_ = create_subscription( + "input/kinematics", 1, std::bind(&ReactionAnalyzerNode::on_vehicle_pose, this, _1), + create_subscription_options(this)); + sub_localization_init_state_ = create_subscription( + "input/localization_initialization_state", rclcpp::QoS(1).transient_local(), + std::bind(&ReactionAnalyzerNode::on_localization_initialization_state, this, _1), + create_subscription_options(this)); + sub_route_state_ = create_subscription( + "input/routing_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_route_state, this, _1), create_subscription_options(this)); + sub_operation_mode_ = create_subscription( + "input/operation_mode_state", rclcpp::QoS{1}.transient_local(), + std::bind(&ReactionAnalyzerNode::on_operation_mode_state, this, _1), + create_subscription_options(this)); + + pub_goal_pose_ = create_publisher("output/goal", rclcpp::QoS(1)); + pub_marker_ = create_publisher("~/debug", 10); + + init_analyzer_variables(); + + if (node_running_mode_ == RunningMode::PlanningControl) { + pub_initial_pose_ = create_publisher( + "output/initialpose", rclcpp::QoS(1)); + + client_change_to_autonomous_ = + create_client("service/change_to_autonomous"); + + } else if (node_running_mode_ == RunningMode::PerceptionPlanning) { + sub_ground_truth_pose_ = create_subscription( + "input/ground_truth_pose", rclcpp::QoS{1}, + std::bind(&ReactionAnalyzerNode::on_ground_truth_pose, this, _1), + create_subscription_options(this)); + } + + topic_publisher_ptr_ = std::make_unique( + this, spawn_object_cmd_, ego_initialized_, spawn_cmd_time_, node_running_mode_, + node_params_.entity_params); + + // initialize the odometry before init the subscriber + odometry_ptr_ = std::make_shared(); + // Load the subscriber to listen the topics for reactions + subscriber_ptr_ = std::make_unique( + this, odometry_ptr_, spawn_object_cmd_, node_params_.entity_params); + + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(node_params_.timer_period)); + timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, std::bind(&ReactionAnalyzerNode::on_timer, this)); +} + +void ReactionAnalyzerNode::on_timer() +{ + mutex_.lock(); + const auto current_odometry_ptr = odometry_ptr_; + const auto initialization_state_ptr = initialization_state_ptr_; + const auto route_state_ptr = current_route_state_ptr_; + const auto operation_mode_ptr = operation_mode_ptr_; + const auto ground_truth_pose_ptr = ground_truth_pose_ptr_; + const auto spawn_cmd_time = spawn_cmd_time_; + mutex_.unlock(); + + // Init the test environment + if (!test_environment_init_time_) { + init_test_env( + initialization_state_ptr, route_state_ptr, operation_mode_ptr, ground_truth_pose_ptr, + current_odometry_ptr); + return; + } + + pub_marker_->publish(entity_debug_marker_); + + // Spawn the obstacle if the conditions are met + spawn_obstacle(current_odometry_ptr->pose.pose.position); + + // If the spawn_cmd_time is not set by pointcloud publishers, don't do anything + if (!spawn_cmd_time) return; + + // Get the reacted messages, if all modules reacted + const auto message_buffers = subscriber_ptr_->get_message_buffers_map(); + + if (message_buffers) { + // if reacted, calculate the results + calculate_results(message_buffers.value(), spawn_cmd_time.value()); + // reset the variables if the iteration is not finished, otherwise write the results + reset(); + } +} + +void ReactionAnalyzerNode::spawn_obstacle(const geometry_msgs::msg::Point & ego_pose) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + rclcpp::Duration time_diff = this->now() - test_environment_init_time_.value(); + if (time_diff > rclcpp::Duration::from_seconds(node_params_.spawn_time_after_init)) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } else { + if ( + tier4_autoware_utils::calcDistance3d(ego_pose, entity_pose_.position) < + node_params_.spawn_distance_threshold) { + if (!spawn_object_cmd_) { + spawn_object_cmd_ = true; + RCLCPP_INFO(this->get_logger(), "Spawn command is sent."); + } + } + } +} + +void ReactionAnalyzerNode::calculate_results( + const std::map & message_buffers, + const rclcpp::Time & spawn_cmd_time) +{ + // Map the reaction times w.r.t its header time to categorize it + PipelineMap pipeline_map; + + { + // set the spawn_cmd_time as the first reaction pair + ReactionPair reaction_pair; + reaction_pair.first = "spawn_cmd_time"; + reaction_pair.second.header.stamp = spawn_cmd_time; + reaction_pair.second.published_stamp = spawn_cmd_time; + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + + for (const auto & [key, variant] : message_buffers) { + ReactionPair reaction_pair; + if (auto * control_message = std::get_if(&variant)) { + if (control_message->second) { + reaction_pair.first = key; + reaction_pair.second = control_message->second.value(); + } + } else if (auto * general_message = std::get_if(&variant)) { + if (general_message->has_value()) { + reaction_pair.first = key; + reaction_pair.second = general_message->value(); + } + } + pipeline_map[reaction_pair.second.header.stamp].emplace_back(reaction_pair); + } + pipeline_map_vector_.emplace_back(pipeline_map); + test_iteration_count_++; +} + +void ReactionAnalyzerNode::init_analyzer_variables() +{ + entity_pose_ = create_entity_pose(node_params_.entity_params); + entity_debug_marker_ = create_polyhedron_marker(node_params_.entity_params); + goal_pose_.pose = pose_params_to_pose(node_params_.goal_pose); + + if (node_running_mode_ == RunningMode::PlanningControl) { + init_pose_.pose.pose = pose_params_to_pose(node_params_.initial_pose); + } +} + +void ReactionAnalyzerNode::init_test_env( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const RouteState::ConstSharedPtr & route_state_ptr, + const OperationModeState::ConstSharedPtr & operation_mode_ptr, + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + const auto current_time = this->now(); + + // Initialize the test environment + constexpr double initialize_call_period = 1.0; // sec + if ( + !last_test_environment_init_request_time_ || + (current_time - last_test_environment_init_request_time_.value()).seconds() >= + initialize_call_period) { + last_test_environment_init_request_time_ = current_time; + + // Pose initialization of the ego + is_initialization_requested = !is_initialization_requested + ? init_ego(initialization_state_ptr, odometry_ptr, current_time) + : is_initialization_requested; + + if ( + is_initialization_requested && + initialization_state_ptr->state != LocalizationInitializationState::INITIALIZED) { + is_initialization_requested = false; + return; + } + + // Check is position initialized accurately, if node is running in perception_planning mode + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + if (!check_ego_init_correctly(ground_truth_pose_ptr, odometry_ptr)) return; + } + + // Set route + is_route_set_ = !is_route_set_ ? set_route(route_state_ptr, current_time) : is_route_set_; + + if (!is_route_set_) { + return; + } + + // if node is running PlanningControl mode, change ego to Autonomous mode. + if (node_running_mode_ == RunningMode::PlanningControl) { + // change to autonomous + if (operation_mode_ptr && operation_mode_ptr->mode != OperationModeState::AUTONOMOUS) { + call_operation_mode_service_without_response(); + } + } + ego_initialized_ = true; + + const bool is_ready = + (is_initialization_requested && is_route_set_ && + (operation_mode_ptr->mode == OperationModeState::AUTONOMOUS || + node_running_mode_ == RunningMode::PerceptionPlanning)); + if (is_ready) { + test_environment_init_time_ = this->now(); + } + } +} + +void ReactionAnalyzerNode::call_operation_mode_service_without_response() +{ + auto req = std::make_shared(); + + RCLCPP_INFO(this->get_logger(), "client request"); + + if (!client_change_to_autonomous_->service_is_ready()) { + RCLCPP_INFO(this->get_logger(), "client is unavailable"); + return; + } + + client_change_to_autonomous_->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + this->get_logger(), "Status: %d, %s", result.get()->status.code, + result.get()->status.message.c_str()); + }); +} + +bool ReactionAnalyzerNode::init_ego( + const LocalizationInitializationState::ConstSharedPtr & initialization_state_ptr, + const Odometry::ConstSharedPtr & odometry_ptr, const rclcpp::Time & current_time) +{ + // Pose initialization of the ego + if (initialization_state_ptr) { + if (node_running_mode_ == RunningMode::PlanningControl) { + // publish initial pose + init_pose_.header.stamp = current_time; + init_pose_.header.frame_id = "map"; + pub_initial_pose_->publish(init_pose_); + RCLCPP_WARN_ONCE(get_logger(), "Initialization position is published. Waiting for init..."); + } + // Wait until odometry_ptr is initialized + if (!odometry_ptr) { + RCLCPP_WARN_ONCE(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::set_route( + const RouteState::ConstSharedPtr & route_state_ptr, const rclcpp::Time & current_time) +{ + if (route_state_ptr) { + if (route_state_ptr->state != RouteState::SET) { + // publish goal pose + goal_pose_.header.stamp = current_time; + goal_pose_.header.frame_id = "map"; + pub_goal_pose_->publish(goal_pose_); + return false; + } + return true; + } + return false; +} + +bool ReactionAnalyzerNode::check_ego_init_correctly( + const PoseStamped::ConstSharedPtr & ground_truth_pose_ptr, + const Odometry::ConstSharedPtr & odometry_ptr) +{ + if (!ground_truth_pose_ptr) { + RCLCPP_WARN( + get_logger(), "Ground truth pose is not received. Waiting for Ground truth pose..."); + return false; + } + if (!odometry_ptr) { + RCLCPP_WARN(get_logger(), "Odometry is not received. Waiting for odometry..."); + return false; + } + + constexpr double deviation_threshold = 0.1; + const auto deviation = + tier4_autoware_utils::calcPoseDeviation(ground_truth_pose_ptr->pose, odometry_ptr->pose.pose); + const bool is_position_initialized_correctly = deviation.longitudinal < deviation_threshold && + deviation.lateral < deviation_threshold && + deviation.yaw < deviation_threshold; + if (!is_position_initialized_correctly) { + RCLCPP_ERROR( + get_logger(), + "Deviation between ground truth position and ego position is high. Node is shutting " + "down. Longitudinal deviation: %f, Lateral deviation: %f, Yaw deviation: %f", + deviation.longitudinal, deviation.lateral, deviation.yaw); + rclcpp::shutdown(); + } + return true; +} + +void ReactionAnalyzerNode::reset() +{ + if (test_iteration_count_ >= node_params_.test_iteration) { + write_results(this, node_params_.output_file_path, node_running_mode_, pipeline_map_vector_); + RCLCPP_INFO(get_logger(), "%zu Tests are finished. Node shutting down.", test_iteration_count_); + rclcpp::shutdown(); + return; + } + // reset the variables + is_initialization_requested = false; + is_route_set_ = false; + test_environment_init_time_ = std::nullopt; + last_test_environment_init_request_time_ = std::nullopt; + spawn_object_cmd_ = false; + if (topic_publisher_ptr_) { + topic_publisher_ptr_->reset(); + } + std::lock_guard lock(mutex_); + spawn_cmd_time_ = std::nullopt; + subscriber_ptr_->reset(); + ego_initialized_ = false; + RCLCPP_INFO(this->get_logger(), "Test - %zu is done, resetting..", test_iteration_count_); +} +} // namespace reaction_analyzer + +#include + +#include + +RCLCPP_COMPONENTS_REGISTER_NODE(reaction_analyzer::ReactionAnalyzerNode) diff --git a/tools/reaction_analyzer/src/subscriber.cpp b/tools/reaction_analyzer/src/subscriber.cpp new file mode 100644 index 0000000000000..8c735c42b9cd5 --- /dev/null +++ b/tools/reaction_analyzer/src/subscriber.cpp @@ -0,0 +1,996 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "subscriber.hpp" + +#include +#include +#include + +namespace reaction_analyzer::subscriber +{ + +SubscriberBase::SubscriberBase( + rclcpp::Node * node, Odometry::ConstSharedPtr & odometry, std::atomic & spawn_object_cmd, + const EntityParams & entity_params) +: node_(node), + odometry_(odometry), + spawn_object_cmd_(spawn_object_cmd), + entity_params_(entity_params) +{ + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // init reaction parameters and chain configuration + init_reaction_chains_and_params(); + init_subscribers(); +} + +void SubscriberBase::init_reaction_chains_and_params() +{ + // Init Chains: get the topic addresses and message types of the modules in chain + { + const auto param_key = std::string("subscriber.reaction_chain"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + TopicConfig tmp; + tmp.node_name = splitted_name.back(); + tmp.topic_address = node_->get_parameter(module_name + ".topic_name").as_string(); + tmp.time_debug_topic_address = + node_->get_parameter_or(module_name + ".time_debug_topic_name", std::string("")); + tmp.message_type = get_subscriber_message_type( + node_->get_parameter(module_name + ".message_type").as_string()); + if (tmp.message_type != SubscriberMessageType::UNKNOWN) { + chain_modules_.emplace_back(tmp); + } else { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for module name: %s, skipping..", + tmp.node_name.c_str()); + } + } + } + + // Init Params: get the parameters for the reaction functions + { + const auto param_key = std::string("subscriber.reaction_params"); + const auto module_names = node_->list_parameters({param_key}, 3).prefixes; + for (const auto & module_name : module_names) { + const auto splitted_name = split(module_name, '.'); + const auto type = get_reaction_type(splitted_name.back()); + switch (type) { + case ReactionType::FIRST_BRAKE: { + reaction_params_.first_brake_params.debug_control_commands = + node_->get_parameter(module_name + ".debug_control_commands").as_bool(); + reaction_params_.first_brake_params.control_cmd_buffer_time_interval = + node_->get_parameter(module_name + ".control_cmd_buffer_time_interval").as_double(); + reaction_params_.first_brake_params.min_number_descending_order_control_cmd = + node_->get_parameter(module_name + ".min_number_descending_order_control_cmd").as_int(); + reaction_params_.first_brake_params.min_jerk_for_brake_cmd = + node_->get_parameter(module_name + ".min_jerk_for_brake_cmd").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), + "First brake parameters are set: debug_control_commands %s, " + "control_cmd_buffer_time_interval %f, min_number_descending_order_control_cmd %zu, " + "min_jerk_for_brake_cmd %f", + reaction_params_.first_brake_params.debug_control_commands ? "true" : "false", + reaction_params_.first_brake_params.control_cmd_buffer_time_interval, + reaction_params_.first_brake_params.min_number_descending_order_control_cmd, + reaction_params_.first_brake_params.min_jerk_for_brake_cmd); + break; + } + case ReactionType::SEARCH_ZERO_VEL: { + reaction_params_.search_zero_vel_params.max_looking_distance = + node_->get_parameter(module_name + ".max_looking_distance").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Zero Vel parameters are set: max_looking_distance %f", + reaction_params_.search_zero_vel_params.max_looking_distance); + break; + } + case ReactionType::SEARCH_ENTITY: { + reaction_params_.search_entity_params.search_radius_offset = + node_->get_parameter(module_name + ".search_radius_offset").as_double(); + RCLCPP_INFO_ONCE( + node_->get_logger(), "Search Entity parameters are set: search_radius_offset %f", + reaction_params_.search_entity_params.search_radius_offset); + break; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown reaction type for module name: %s, skipping..", + splitted_name.back().c_str()); + break; + } + } + } + + // Init variables + { + entity_pose_ = create_entity_pose(entity_params_); + entity_search_radius_ = calculate_entity_search_radius(entity_params_) + + reaction_params_.search_entity_params.search_radius_offset; + } +} + +bool SubscriberBase::init_subscribers() +{ + if (chain_modules_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No module to initialize subscribers, failed."); + return false; + } + + for (const auto & module : chain_modules_) { + if (module.message_type == SubscriberMessageType::UNKNOWN) { + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + module.node_name.c_str()); + } + auto subscriber_var = get_subscriber_variable(module); + if (!subscriber_var) { + RCLCPP_ERROR( + node_->get_logger(), "Failed to initialize subscriber for module name: %s", + module.node_name.c_str()); + return false; + } + subscriber_variables_map_[module.node_name] = std::move(subscriber_var.value()); + } + return true; +} + +std::optional> SubscriberBase::get_message_buffers_map() +{ + std::lock_guard lock(mutex_); + if (message_buffers_.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No message buffers are initialized."); + return std::nullopt; + } + + // Check all reacted or not + bool all_reacted = true; + for (const auto & [key, variant] : message_buffers_) { + if (auto * control_message = std::get_if(&variant)) { + if (!control_message->second) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str()); + all_reacted = false; + } + } else if (auto * general_message = std::get_if(&variant)) { + if (!general_message->has_value()) { + RCLCPP_WARN_THROTTLE( + node_->get_logger(), *node_->get_clock(), 1000, "Waiting for %s to react", key.c_str()); + all_reacted = false; + } + } + } + if (!all_reacted) { + return std::nullopt; + } + return message_buffers_; +} + +void SubscriberBase::reset() +{ + std::lock_guard lock(mutex_); + message_buffers_.clear(); +} + +// Callbacks + +void SubscriberBase::on_control_command( + const std::string & node_name, const AckermannControlCommand::ConstSharedPtr & msg_ptr) +{ + std::lock_guard lock(mutex_); + auto & variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + ControlCommandBuffer buffer(std::vector{*msg_ptr}, std::nullopt); + variant = buffer; + } + auto & cmd_buffer = std::get(variant); + if (cmd_buffer.second) { + // reacted + return; + } + set_control_command_to_buffer(cmd_buffer.first, *msg_ptr); + if (!spawn_object_cmd_) return; + + const auto brake_idx = find_first_brake_idx(cmd_buffer.first); + if (brake_idx) { + const auto brake_cmd = cmd_buffer.first.at(brake_idx.value()); + + // TODO(brkay54): update here if message_filters package add support for the messages which + // does not have header + const auto & subscriber_variant = + std::get>(subscriber_variables_map_[node_name]); + + // check if the cache was initialized or not, if there is, use it to set the published time + if (subscriber_variant.cache_) { + // use cache to get the published time + const auto published_time_vec = + subscriber_variant.cache_->getSurroundingInterval(brake_cmd.stamp, node_->now()); + if (!published_time_vec.empty()) { + for (const auto & published_time : published_time_vec) { + if (published_time->header.stamp == brake_cmd.stamp) { + cmd_buffer.second = *published_time; + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + return; + } + } + RCLCPP_ERROR( + node_->get_logger(), "Published time couldn't found for the node: %s", node_name.c_str()); + + } else { + RCLCPP_ERROR( + node_->get_logger(), "Published time vector is empty for the node: %s", + node_name.c_str()); + } + } else { + cmd_buffer.second->header.stamp = brake_cmd.stamp; + cmd_buffer.second->published_stamp = brake_cmd.stamp; + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + } + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + const auto nearest_objects_seg_idx = + motion_utils::findNearestIndex(msg_ptr->points, entity_pose_.position); + + const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex( + msg_ptr->points, nearest_seg_idx, nearest_objects_seg_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_trajectory( + const std::string & node_name, const Trajectory::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + const auto current_odometry_ptr = odometry_; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!current_odometry_ptr || !spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex( + msg_ptr->points, current_odometry_ptr->pose.pose.position); + + // find the target index which we will search for zero velocity + auto tmp_target_idx = get_index_after_distance( + *msg_ptr, nearest_seg_idx, reaction_params_.search_zero_vel_params.max_looking_distance); + if (tmp_target_idx == msg_ptr->points.size() - 1) { + tmp_target_idx = msg_ptr->points.size() - 2; // Last trajectory points might be zero velocity + } + const auto target_idx = tmp_target_idx; + const auto zero_vel_idx = + motion_utils::searchZeroVelocityIndex(msg_ptr->points, nearest_seg_idx, target_idx); + + if (zero_vel_idx) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_pointcloud( + const std::string & node_name, const PointCloud2::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if (!spawn_object_cmd_ || std::get(variant).has_value()) { + return; + } + + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *msg_ptr, transformed_points); + + pcl::PointCloud pcl_pointcloud; + pcl::fromROSMsg(transformed_points, pcl_pointcloud); + + if (search_pointcloud_near_pose(pcl_pointcloud, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_predicted_objects( + const std::string & node_name, const PredictedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_predicted_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted without published time", node_name.c_str()); + // set header time + auto & buffer = std::get(variant); + buffer->header.stamp = msg_ptr->header.stamp; + buffer->published_stamp = msg_ptr->header.stamp; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_detected_objects( + const std::string & node_name, const DetectedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + // transform objects + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "map", msg_ptr->header.frame_id, node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Failed to look up transform from " << msg_ptr->header.frame_id << " to map"); + return; + } + + DetectedObjects output_objs; + output_objs = *msg_ptr; + for (auto & obj : output_objs.objects) { + geometry_msgs::msg::PoseStamped output_stamped; + geometry_msgs::msg::PoseStamped input_stamped; + input_stamped.pose = obj.kinematics.pose_with_covariance.pose; + tf2::doTransform(input_stamped, output_stamped, transform_stamped); + obj.kinematics.pose_with_covariance.pose = output_stamped.pose; + } + if (search_detected_objects_near_pose(output_objs, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +void SubscriberBase::on_tracked_objects( + const std::string & node_name, const TrackedObjects::ConstSharedPtr & msg_ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) +{ + mutex_.lock(); + auto variant = message_buffers_[node_name]; + if (!std::holds_alternative(variant)) { + MessageBuffer buffer(std::nullopt); + variant = buffer; + message_buffers_[node_name] = variant; + } + mutex_.unlock(); + if ( + !spawn_object_cmd_ || msg_ptr->objects.empty() || + std::get(variant).has_value()) { + return; + } + + if (search_tracked_objects_near_pose(*msg_ptr, entity_pose_, entity_search_radius_)) { + RCLCPP_INFO(node_->get_logger(), "%s reacted with published time", node_name.c_str()); + // set published time + auto & buffer = std::get(variant); + buffer = *published_time_ptr; + mutex_.lock(); + message_buffers_[node_name] = variant; + mutex_.unlock(); + } +} + +std::optional SubscriberBase::get_subscriber_variable( + const TopicConfig & topic_config) +{ + switch (topic_config.message_type) { + case SubscriberMessageType::ACKERMANN_CONTROL_COMMAND: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + // If not empty, user should define a time debug topic + // NOTE: Because message_filters package does not support the messages without headers, we + // can not use the synchronizer. After we reacted, we are going to use the cache + // of the both PublishedTime and AckermannControlCommand subscribers to find the messages + // which have same header time. + + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + constexpr int cache_size = 5; + subscriber_variable.cache_ = std::make_unique>( + *subscriber_variable.sub2_, cache_size); + + } else { + std::function callback = + [this, topic_config](const AckermannControlCommand::ConstSharedPtr & ptr) { + this->on_control_command(topic_config.node_name, ptr); + }; + + subscriber_variable.sub1_ = + std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRAJECTORY: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const Trajectory::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_trajectory(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const Trajectory::ConstSharedPtr & msg) { + this->on_trajectory(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::POINTCLOUD2: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PointCloud2::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_pointcloud(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::SensorDataQoS().get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PointCloud2::ConstSharedPtr & msg) { + this->on_pointcloud(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::PREDICTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const PredictedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_predicted_objects(topic_config.node_name, ptr, published_time_ptr); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const PredictedObjects::ConstSharedPtr & msg) { + this->on_predicted_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::DETECTED_OBJECTS: { + SubscriberVariables subscriber_variable; + + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const DetectedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_detected_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const DetectedObjects::ConstSharedPtr & msg) { + this->on_detected_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + case SubscriberMessageType::TRACKED_OBJECTS: { + SubscriberVariables subscriber_variable; + if (!topic_config.time_debug_topic_address.empty()) { + std::function + callback = [this, topic_config]( + const TrackedObjects::ConstSharedPtr & ptr, + const PublishedTime::ConstSharedPtr & published_time_ptr) { + this->on_tracked_objects(topic_config.node_name, ptr, published_time_ptr); + }; + + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + subscriber_variable.sub2_ = std::make_unique>( + node_, topic_config.time_debug_topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.synchronizer_ = std::make_unique< + message_filters::Synchronizer::ExactTimePolicy>>( + SubscriberVariables::ExactTimePolicy(10), *subscriber_variable.sub1_, + *subscriber_variable.sub2_); + + subscriber_variable.synchronizer_->registerCallback( + std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + } else { + std::function callback = + [this, topic_config](const TrackedObjects::ConstSharedPtr & msg) { + this->on_tracked_objects(topic_config.node_name, msg); + }; + subscriber_variable.sub1_ = std::make_unique>( + node_, topic_config.topic_address, rclcpp::QoS(1).get_rmw_qos_profile(), + create_subscription_options(node_)); + + subscriber_variable.sub1_->registerCallback(std::bind(callback, std::placeholders::_1)); + RCLCPP_WARN( + node_->get_logger(), + "PublishedTime will not be used for node name: %s. Header timestamp will be used for " + "calculations", + topic_config.node_name.c_str()); + } + return subscriber_variable; + } + default: + RCLCPP_WARN( + node_->get_logger(), "Unknown message type for topic_config name: %s, skipping..", + topic_config.node_name.c_str()); + return std::nullopt; + } +} + +std::optional SubscriberBase::find_first_brake_idx( + const std::vector & cmd_array) +{ + if ( + cmd_array.size() < + reaction_params_.first_brake_params.min_number_descending_order_control_cmd) { + RCLCPP_WARN_ONCE( + node_->get_logger(), + "Control command buffer size is less than the minimum required size for first brake check"); + return {}; + } + + for (size_t i = 0; + i < cmd_array.size() - + reaction_params_.first_brake_params.min_number_descending_order_control_cmd + 1; + ++i) { + size_t decreased_cmd_counter = 1; // because # of the decreased cmd = iteration + 1 + for (size_t j = i; j < cmd_array.size() - 1; ++j) { + const auto & cmd_first = cmd_array.at(j).longitudinal; + const auto & cmd_second = cmd_array.at(j + 1).longitudinal; + constexpr double jerk_time_epsilon = 0.001; + const auto jerk = + abs(cmd_second.acceleration - cmd_first.acceleration) / + std::max( + (rclcpp::Time(cmd_second.stamp) - rclcpp::Time(cmd_first.stamp)).seconds(), + jerk_time_epsilon); + + if ( + (cmd_second.acceleration < cmd_first.acceleration) && + (jerk > reaction_params_.first_brake_params.min_jerk_for_brake_cmd)) { + decreased_cmd_counter++; + } else { + break; + } + } + if ( + decreased_cmd_counter < + static_cast( + reaction_params_.first_brake_params.min_number_descending_order_control_cmd)) + continue; + if (reaction_params_.first_brake_params.debug_control_commands) { + std::stringstream ss; + + // debug print to show the first brake command in the all control commands + for (size_t k = 0; k < cmd_array.size(); ++k) { + if (k == i + 1) { + ss << "First Brake(" << cmd_array.at(k).longitudinal.acceleration << ") "; + } else { + ss << cmd_array.at(k).longitudinal.acceleration << " "; + } + } + + RCLCPP_INFO(node_->get_logger(), "%s", ss.str().c_str()); + } + return i + 1; + } + return {}; +} + +void SubscriberBase::set_control_command_to_buffer( + std::vector & buffer, const AckermannControlCommand & cmd) const +{ + const auto last_cmd_time = cmd.stamp; + if (!buffer.empty()) { + for (auto itr = buffer.begin(); itr != buffer.end();) { + const auto expired = (rclcpp::Time(last_cmd_time) - rclcpp::Time(itr->stamp)).seconds() > + reaction_params_.first_brake_params.control_cmd_buffer_time_interval; + + if (expired) { + itr = buffer.erase(itr); + continue; + } + + itr++; + } + } + buffer.emplace_back(cmd); +} +} // namespace reaction_analyzer::subscriber diff --git a/tools/reaction_analyzer/src/topic_publisher.cpp b/tools/reaction_analyzer/src/topic_publisher.cpp new file mode 100644 index 0000000000000..8a66bf9e33911 --- /dev/null +++ b/tools/reaction_analyzer/src/topic_publisher.cpp @@ -0,0 +1,536 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "topic_publisher.hpp" + +#include +#include + +namespace reaction_analyzer::topic_publisher +{ + +TopicPublisher::TopicPublisher( + rclcpp::Node * node, std::atomic & spawn_object_cmd, std::atomic & ego_initialized, + std::optional & spawn_cmd_time, const RunningMode & node_running_mode, + const EntityParams & entity_params) +: node_(node), + node_running_mode_(node_running_mode), + spawn_object_cmd_(spawn_object_cmd), + ego_initialized_(ego_initialized), + entity_params_(entity_params), + spawn_cmd_time_(spawn_cmd_time) +{ + if (node_running_mode_ == RunningMode::PerceptionPlanning) { + // get perception_planning mode parameters + topic_publisher_params_.path_bag_with_object = + node_->get_parameter("topic_publisher.path_bag_with_object").as_string(); + topic_publisher_params_.path_bag_without_object = + node_->get_parameter("topic_publisher.path_bag_without_object").as_string(); + topic_publisher_params_.pointcloud_publisher_type = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_type") + .as_string(); + topic_publisher_params_.pointcloud_publisher_period = + node_->get_parameter("topic_publisher.pointcloud_publisher.pointcloud_publisher_period") + .as_double(); + topic_publisher_params_.publish_only_pointcloud_with_object = + node_ + ->get_parameter("topic_publisher.pointcloud_publisher.publish_only_pointcloud_with_object") + .as_bool(); + + // set pointcloud publisher type + if (topic_publisher_params_.pointcloud_publisher_type == "sync_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_header_sync_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER; + } else if (topic_publisher_params_.pointcloud_publisher_type == "async_publish") { + pointcloud_publisher_type_ = PointcloudPublisherType::ASYNC_PUBLISHER; + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid pointcloud_publisher_type"); + rclcpp::shutdown(); + return; + } + + // Init the publishers which will read the messages from the rosbag + init_rosbag_publishers(); + } else if (node_running_mode_ == RunningMode::PlanningControl) { + // init tf + tf_buffer_ = std::make_shared(node_->get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + // get parameters + topic_publisher_params_.dummy_perception_publisher_period = + node_->get_parameter("topic_publisher.dummy_perception_publisher_period").as_double(); + topic_publisher_params_.spawned_pointcloud_sampling_distance = + node_->get_parameter("topic_publisher.spawned_pointcloud_sampling_distance").as_double(); + + // init the messages that will be published to spawn the object + entity_pointcloud_ptr_ = create_entity_pointcloud_ptr( + entity_params_, topic_publisher_params_.spawned_pointcloud_sampling_distance); + predicted_objects_ptr_ = create_entity_predicted_objects_ptr(entity_params_); + + // init the publishers + pub_pointcloud_ = + node_->create_publisher("output/pointcloud", rclcpp::SensorDataQoS()); + pub_predicted_objects_ = + node_->create_publisher("output/objects", rclcpp::QoS(1)); + + // init dummy perception publisher + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.dummy_perception_publisher_period)); + + dummy_perception_timer_ = rclcpp::create_timer( + node_, node_->get_clock(), period_ns, + std::bind(&TopicPublisher::dummy_perception_publisher, this)); + } else { + RCLCPP_ERROR(node_->get_logger(), "Invalid running mode"); + rclcpp::shutdown(); + return; + } +} + +void TopicPublisher::pointcloud_messages_sync_publisher(const PointcloudPublisherType type) +{ + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + + switch (type) { + case PointcloudPublisherType::SYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + accessor.publish_with_current_time( + *publisher_var_pair.second.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + case PointcloudPublisherType::ASYNC_HEADER_SYNC_PUBLISHER: { + PublisherVarAccessor accessor; + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + size_t counter = 0; + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto header_time = + current_time - std::chrono::nanoseconds(counter * phase_dif.count()); + accessor.publish_with_current_time( + *publisher_var_pair.second.first, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *publisher_var_pair.second.second, header_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + counter++; + } + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } + break; + } + default: + break; + } +} + +void TopicPublisher::pointcloud_messages_async_publisher( + const std::pair< + std::shared_ptr>, + std::shared_ptr>> & lidar_output_pair_) +{ + PublisherVarAccessor accessor; + const auto current_time = node_->now(); + const bool is_object_spawned = spawn_object_cmd_; + accessor.publish_with_current_time( + *lidar_output_pair_.first, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + accessor.publish_with_current_time( + *lidar_output_pair_.second, current_time, + topic_publisher_params_.publish_only_pointcloud_with_object || is_object_spawned); + + if (is_object_spawned && !is_object_spawned_message_published_) { + is_object_spawned_message_published_ = true; + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + } +} + +void TopicPublisher::generic_message_publisher(const std::string & topic_name) +{ + PublisherVarAccessor accessor; + const bool is_object_spawned = spawn_object_cmd_; + const auto current_time = node_->now(); + const auto & publisher_variant = topic_publisher_map_[topic_name]; + + std::visit( + [&](const auto & var) { + accessor.publish_with_current_time(var, current_time, is_object_spawned); + }, + publisher_variant); +} + +void TopicPublisher::dummy_perception_publisher() +{ + if (!ego_initialized_) { + return; // do not publish anything if ego is not initialized + } + if (!spawn_object_cmd_) { + // do not spawn it, send empty pointcloud + pcl::PointCloud pcl_empty; + PointCloud2 empty_pointcloud; + PredictedObjects empty_predicted_objects; + pcl::toROSMsg(pcl_empty, empty_pointcloud); + + const auto current_time = node_->now(); + empty_pointcloud.header.frame_id = "base_link"; + empty_pointcloud.header.stamp = current_time; + + empty_predicted_objects.header.frame_id = "map"; + empty_predicted_objects.header.stamp = current_time; + + pub_pointcloud_->publish(empty_pointcloud); + pub_predicted_objects_->publish(empty_predicted_objects); + } else { + // transform pointcloud + geometry_msgs::msg::TransformStamped transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + "base_link", "map", node_->now(), rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Failed to look up transform from map to base_link"); + return; + } + + // transform by using eigen matrix + PointCloud2 transformed_points{}; + const Eigen::Matrix4f affine_matrix = + tf2::transformToEigen(transform_stamped.transform).matrix().cast(); + pcl_ros::transformPointCloud(affine_matrix, *entity_pointcloud_ptr_, transformed_points); + const auto current_time = node_->now(); + + transformed_points.header.frame_id = "base_link"; + transformed_points.header.stamp = current_time; + + predicted_objects_ptr_->header.frame_id = "map"; + predicted_objects_ptr_->header.stamp = current_time; + + pub_pointcloud_->publish(transformed_points); + pub_predicted_objects_->publish(*predicted_objects_ptr_); + if (!is_object_spawned_message_published_) { + mutex_.lock(); + spawn_cmd_time_ = current_time; + mutex_.unlock(); + is_object_spawned_message_published_ = true; + } + } +} + +void TopicPublisher::reset() +{ + is_object_spawned_message_published_ = false; +} + +void TopicPublisher::init_rosbag_publishers() +{ + // read messages without object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_without_object, true); + + // read messages with object + init_rosbag_publisher_buffer(topic_publisher_params_.path_bag_with_object, false); + + // before create publishers and timers, check all the messages are correctly initialized with + // their conjugate messages. + if (check_publishers_initialized_correctly()) { + RCLCPP_INFO(node_->get_logger(), "Messages are initialized correctly"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Messages are not initialized correctly"); + rclcpp::shutdown(); + } + set_publishers_and_timers_to_variable(); +} + +template +void TopicPublisher::set_message( + const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message, + const bool is_empty_area_message) +{ + rclcpp::Serialization serialization; + rclcpp::SerializedMessage extracted_serialized_msg(*bag_message.serialized_data); + + // Deserialize the message + auto deserialized_message = std::make_shared(); + serialization.deserialize_message(&extracted_serialized_msg, &*deserialized_message); + auto & publisher_variant = topic_publisher_map_[topic_name]; + + if (!std::holds_alternative>(publisher_variant)) { + publisher_variant = PublisherVariables{}; + } + + auto & publisher_variable = std::get>(publisher_variant); + + if (is_empty_area_message) { + if (!publisher_variable.empty_area_message) { + publisher_variable.empty_area_message = deserialized_message; + } + } else { + if (!publisher_variable.object_spawned_message) { + publisher_variable.object_spawned_message = deserialized_message; + } + } +} + +void TopicPublisher::set_period(const std::map> & time_map) +{ + for (auto & topic_pair : time_map) { + auto timestamps_tmp = topic_pair.second; + + // Sort the timestamps + std::sort(timestamps_tmp.begin(), timestamps_tmp.end()); + + // Then proceed with the frequency calculation + std::string topic_name = topic_pair.first; + if (timestamps_tmp.size() > 1) { + int64_t total_time_diff_ns = 0; + + // Accumulate the differences in nanoseconds + for (size_t i = 1; i < timestamps_tmp.size(); ++i) { + total_time_diff_ns += (timestamps_tmp[i] - timestamps_tmp[i - 1]).nanoseconds(); + } + + // Conversion to std::chrono::milliseconds + auto total_duration_ns = std::chrono::nanoseconds(total_time_diff_ns); + auto period_ms = std::chrono::duration_cast(total_duration_ns) / + (timestamps_tmp.size() - 1); + + PublisherVariablesVariant & publisher_variant = topic_publisher_map_[topic_name]; + PublisherVarAccessor accessor; + + std::visit([&](auto & var) { accessor.set_period(var, period_ms); }, publisher_variant); + } + } +} + +void TopicPublisher::set_publishers_and_timers_to_variable() +{ + std::map> + pointcloud_variables_map; // temp map for pointcloud publishers + + // initialize timers and message publishers except pointcloud messages + for (auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + const auto & topic_ref = topic_name; + const auto period_ns = std::chrono::duration( + std::visit([&](const auto & var) { return accessor.get_period(var); }, variant)); + + // Dynamically create the correct publisher type based on the topic + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr ( + std::is_same_v || + std::is_same_v || + std::is_same_v) { + var.publisher = node_->create_publisher(topic_ref, rclcpp::SensorDataQoS()); + } else { + // For other message types, use the QoS setting depth of 1 + var.publisher = node_->create_publisher(topic_ref, rclcpp::QoS(1)); + } + }, + variant); + + // Conditionally create the timer based on the message type, if message type is not + // PointCloud2 + std::visit( + [&](auto & var) { + using MessageType = typename decltype(var.empty_area_message)::element_type; + + if constexpr (!std::is_same_v) { + var.timer = node_->create_wall_timer( + period_ns, [this, topic_ref]() { this->generic_message_publisher(topic_ref); }); + } else { + // For PointCloud2, Store the variables in a temporary map + pointcloud_variables_map[topic_ref] = var; + } + }, + variant); + } + + // To be able to publish pointcloud messages with async, I need to create a timer for each lidar + // output. So different operations are needed for pointcloud messages. + set_timers_for_pointcloud_msgs(pointcloud_variables_map); +} + +void TopicPublisher::set_timers_for_pointcloud_msgs( + const std::map> & pointcloud_variables_map) +{ + // Set the point cloud publisher timers + if (pointcloud_variables_map.empty()) { + RCLCPP_ERROR(node_->get_logger(), "No pointcloud publishers found!"); + rclcpp::shutdown(); + } + + // Arrange the PointCloud2 variables w.r.t. the lidars' name + for (auto & [topic_name, pointcloud_variant] : pointcloud_variables_map) { + const auto lidar_name = split(topic_name, '/').at(3); + + if (lidar_pub_variable_pair_map_.find(lidar_name) == lidar_pub_variable_pair_map_.end()) { + lidar_pub_variable_pair_map_[lidar_name] = std::make_pair( + std::make_shared>(pointcloud_variant), nullptr); + } else { + if (lidar_pub_variable_pair_map_[lidar_name].second) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Lidar name: " << lidar_name << " is already used by another pointcloud publisher"); + rclcpp::shutdown(); + } + lidar_pub_variable_pair_map_[lidar_name].second = + std::make_shared>(pointcloud_variant); + } + } + + // Create the timer(s) to publish PointCloud2 Messages + const auto period_pointcloud_ns = std::chrono::duration_cast( + std::chrono::duration(topic_publisher_params_.pointcloud_publisher_period)); + + if (pointcloud_publisher_type_ != PointcloudPublisherType::ASYNC_PUBLISHER) { + // Create 1 timer to publish all PointCloud2 messages + pointcloud_sync_publish_timer_ = node_->create_wall_timer(period_pointcloud_ns, [this]() { + this->pointcloud_messages_sync_publisher(this->pointcloud_publisher_type_); + }); + } else { + // Create multiple timers which will run with a phase difference + const auto phase_dif = period_pointcloud_ns / lidar_pub_variable_pair_map_.size(); + + // Create a timer to create phase difference bw timers which will be created for each lidar + // topics + auto one_shot_timer = node_->create_wall_timer(phase_dif, [this, period_pointcloud_ns]() { + for (const auto & publisher_var_pair : lidar_pub_variable_pair_map_) { + const auto & lidar_name = publisher_var_pair.first; + const auto & publisher_var = publisher_var_pair.second; + if ( + pointcloud_publish_timers_map_.find(lidar_name) == pointcloud_publish_timers_map_.end()) { + auto periodic_timer = node_->create_wall_timer( + period_pointcloud_ns, + [this, publisher_var]() { this->pointcloud_messages_async_publisher(publisher_var); }); + pointcloud_publish_timers_map_[lidar_name] = periodic_timer; + return; + } + } + one_shot_timer_shared_ptr_->cancel(); + }); + one_shot_timer_shared_ptr_ = one_shot_timer; + } +} + +bool TopicPublisher::check_publishers_initialized_correctly() +{ + // check messages are correctly initialized or not from rosbags + for (const auto & [topic_name, variant] : topic_publisher_map_) { + PublisherVarAccessor accessor; + auto empty_area_message = + std::visit([&](const auto & var) { return accessor.get_empty_area_message(var); }, variant); + auto object_spawned_message = std::visit( + [&](const auto & var) { return accessor.get_object_spawned_message(var); }, variant); + + if (!empty_area_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Empty area message couldn't found in the topic named: " << topic_name); + return false; + } + if (!object_spawned_message) { + RCLCPP_ERROR_STREAM( + node_->get_logger(), + "Object spawned message couldn't found in the topic named: " << topic_name); + return false; + } + } + return true; +} + +void TopicPublisher::init_rosbag_publisher_buffer( + const std::string & bag_path, const bool is_empty_area_message) +{ + rosbag2_cpp::Reader reader; + + try { + reader.open(bag_path); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(node_->get_logger(), "Error opening bag file: " << e.what()); + rclcpp::shutdown(); + return; + } + + const auto & topics = reader.get_metadata().topics_with_message_count; + + while (reader.has_next()) { + auto bag_message = reader.read_next(); + const auto current_topic = bag_message->topic_name; + + const auto message_type = get_publisher_message_type_for_topic(topics, current_topic); + + if (message_type == PublisherMessageType::UNKNOWN) { + continue; + } + if (message_type == PublisherMessageType::CAMERA_INFO) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMAGE) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POINTCLOUD2) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::POSE_STAMPED) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::ODOMETRY) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::IMU) { + set_message(current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::CONTROL_MODE_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::GEAR_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::HAZARD_LIGHTS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::STEERING_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::TURN_INDICATORS_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } else if (message_type == PublisherMessageType::VELOCITY_REPORT) { + set_message( + current_topic, *bag_message, is_empty_area_message); + } + } + reader.close(); +} +} // namespace reaction_analyzer::topic_publisher diff --git a/tools/reaction_analyzer/src/utils.cpp b/tools/reaction_analyzer/src/utils.cpp new file mode 100644 index 0000000000000..4a6110322440e --- /dev/null +++ b/tools/reaction_analyzer/src/utils.cpp @@ -0,0 +1,589 @@ +// Copyright 2024 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utils.hpp" + +namespace reaction_analyzer +{ +SubscriberMessageType get_subscriber_message_type(const std::string & message_type) +{ + if (message_type == "autoware_auto_control_msgs/msg/AckermannControlCommand") { + return SubscriberMessageType::ACKERMANN_CONTROL_COMMAND; + } + if (message_type == "autoware_auto_planning_msgs/msg/Trajectory") { + return SubscriberMessageType::TRAJECTORY; + } + if (message_type == "sensor_msgs/msg/PointCloud2") { + return SubscriberMessageType::POINTCLOUD2; + } + if (message_type == "autoware_auto_perception_msgs/msg/PredictedObjects") { + return SubscriberMessageType::PREDICTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/DetectedObjects") { + return SubscriberMessageType::DETECTED_OBJECTS; + } + if (message_type == "autoware_auto_perception_msgs/msg/TrackedObjects") { + return SubscriberMessageType::TRACKED_OBJECTS; + } + return SubscriberMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type(const std::string & message_type) +{ + if (message_type == "sensor_msgs/msg/PointCloud2") { + return PublisherMessageType::POINTCLOUD2; + } + if (message_type == "sensor_msgs/msg/CameraInfo") { + return PublisherMessageType::CAMERA_INFO; + } + if (message_type == "sensor_msgs/msg/Image") { + return PublisherMessageType::IMAGE; + } + if (message_type == "geometry_msgs/msg/PoseWithCovarianceStamped") { + return PublisherMessageType::POSE_WITH_COVARIANCE_STAMPED; + } + if (message_type == "geometry_msgs/msg/PoseStamped") { + return PublisherMessageType::POSE_STAMPED; + } + if (message_type == "nav_msgs/msg/Odometry") { + return PublisherMessageType::ODOMETRY; + } + if (message_type == "sensor_msgs/msg/Imu") { + return PublisherMessageType::IMU; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/ControlModeReport") { + return PublisherMessageType::CONTROL_MODE_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/GearReport") { + return PublisherMessageType::GEAR_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/HazardLightsReport") { + return PublisherMessageType::HAZARD_LIGHTS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/SteeringReport") { + return PublisherMessageType::STEERING_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport") { + return PublisherMessageType::TURN_INDICATORS_REPORT; + } + if (message_type == "autoware_auto_vehicle_msgs/msg/VelocityReport") { + return PublisherMessageType::VELOCITY_REPORT; + } + return PublisherMessageType::UNKNOWN; +} + +PublisherMessageType get_publisher_message_type_for_topic( + const std::vector & topics, const std::string & topic_name) +{ + auto it = std::find_if(topics.begin(), topics.end(), [&topic_name](const auto & topic) { + return topic.topic_metadata.name == topic_name; + }); + if (it != topics.end()) { + return get_publisher_message_type(it->topic_metadata.type); // Return the message type if found + } + return PublisherMessageType::UNKNOWN; +} + +ReactionType get_reaction_type(const std::string & reaction_type) +{ + if (reaction_type == "first_brake_params") { + return ReactionType::FIRST_BRAKE; + } + if (reaction_type == "search_zero_vel_params") { + return ReactionType::SEARCH_ZERO_VEL; + } + if (reaction_type == "search_entity_params") { + return ReactionType::SEARCH_ENTITY; + } + return ReactionType::UNKNOWN; +} + +rclcpp::SubscriptionOptions create_subscription_options(rclcpp::Node * node) +{ + rclcpp::CallbackGroup::SharedPtr callback_group = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group; + + return sub_opt; +} + +visualization_msgs::msg::Marker create_polyhedron_marker(const EntityParams & params) +{ + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(0); + marker.ns = "entity"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::LINE_STRIP; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = params.x; + marker.pose.position.y = params.y; + marker.pose.position.z = params.z; + + tf2::Quaternion quaternion; + quaternion.setRPY( + tier4_autoware_utils::deg2rad(params.roll), tier4_autoware_utils::deg2rad(params.pitch), + tier4_autoware_utils::deg2rad(params.yaw)); + marker.pose.orientation = tf2::toMsg(quaternion); + + marker.scale.x = 0.1; // Line width + + marker.color.a = 1.0; // Alpha + marker.color.r = 1.0; + marker.color.g = 0.0; + marker.color.b = 0.0; + + // Define the 8 corners of the polyhedron + geometry_msgs::msg::Point p1, p2, p3, p4, p5, p6, p7, p8; + + p1.x = params.x_l / 2.0; + p1.y = params.y_l / 2.0; + p1.z = params.z_l / 2.0; + p2.x = -params.x_l / 2.0; + p2.y = params.y_l / 2.0; + p2.z = params.z_l / 2.0; + p3.x = -params.x_l / 2.0; + p3.y = -params.y_l / 2.0; + p3.z = params.z_l / 2.0; + p4.x = params.x_l / 2.0; + p4.y = -params.y_l / 2.0; + p4.z = params.z_l / 2.0; + p5.x = params.x_l / 2.0; + p5.y = params.y_l / 2.0; + p5.z = -params.z_l / 2.0; + p6.x = -params.x_l / 2.0; + p6.y = params.y_l / 2.0; + p6.z = -params.z_l / 2.0; + p7.x = -params.x_l / 2.0; + p7.y = -params.y_l / 2.0; + p7.z = -params.z_l / 2.0; + p8.x = params.x_l / 2.0; + p8.y = -params.y_l / 2.0; + p8.z = -params.z_l / 2.0; + + // Add points to the marker + marker.points.push_back(p1); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p4); + marker.points.push_back(p1); + + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p7); + marker.points.push_back(p8); + marker.points.push_back(p5); + + marker.points.push_back(p1); + marker.points.push_back(p5); + marker.points.push_back(p6); + marker.points.push_back(p2); + marker.points.push_back(p3); + marker.points.push_back(p7); + marker.points.push_back(p4); + marker.points.push_back(p8); + + return marker; +} + +std::vector split(const std::string & str, char delimiter) +{ + std::vector elements; + std::stringstream ss(str); + std::string item; + while (std::getline(ss, item, delimiter)) { + elements.push_back(item); + } + return elements; +} + +bool does_folder_exist(const std::string & path) +{ + return std::filesystem::exists(path) && std::filesystem::is_directory(path); +} + +size_t get_index_after_distance( + const Trajectory & traj, const size_t curr_id, const double distance) +{ + const TrajectoryPoint & curr_p = traj.points.at(curr_id); + + size_t target_id = curr_id; + double current_distance = 0.0; + for (size_t traj_id = curr_id + 1; traj_id < traj.points.size(); ++traj_id) { + current_distance = tier4_autoware_utils::calcDistance3d(traj.points.at(traj_id), curr_p); + if (current_distance >= distance) { + break; + } + target_id = traj_id; + } + return target_id; +} + +double calculate_time_diff_ms(const rclcpp::Time & start, const rclcpp::Time & end) +{ + const auto duration = end - start; + + const auto duration_ns = duration.to_chrono(); + return static_cast(duration_ns.count()) / 1e6; +} + +TimestampedReactionPairsVector convert_pipeline_map_to_sorted_vector( + const PipelineMap & pipelineMap) +{ + std::vector>> sorted_vector; + + for (const auto & entry : pipelineMap) { + auto sorted_reactions = entry.second; + // Sort the vector of ReactionPair based on the published stamp + std::sort( + sorted_reactions.begin(), sorted_reactions.end(), + [](const ReactionPair & a, const ReactionPair & b) { + return rclcpp::Time(a.second.published_stamp) < rclcpp::Time(b.second.published_stamp); + }); + + // Add to the vector as a tuple + sorted_vector.emplace_back(std::make_tuple(entry.first, sorted_reactions)); + } + + // Sort the vector of tuples by rclcpp::Time + std::sort(sorted_vector.begin(), sorted_vector.end(), [](const auto & a, const auto & b) { + return std::get<0>(a) < std::get<0>(b); + }); + + return sorted_vector; +} + +unique_identifier_msgs::msg::UUID generate_uuid_msg(const std::string & input) +{ + static auto generate_uuid = boost::uuids::name_generator(boost::uuids::random_generator()()); + const auto uuid = generate_uuid(input); + + unique_identifier_msgs::msg::UUID uuid_msg; + std::copy(uuid.begin(), uuid.end(), uuid_msg.uuid.begin()); + return uuid_msg; +} + +geometry_msgs::msg::Pose create_entity_pose(const EntityParams & entity_params) +{ + geometry_msgs::msg::Pose entity_pose; + entity_pose.position.x = entity_params.x; + entity_pose.position.y = entity_params.y; + entity_pose.position.z = entity_params.z; + + tf2::Quaternion entity_q_orientation; + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + entity_pose.orientation = tf2::toMsg(entity_q_orientation); + return entity_pose; +} + +geometry_msgs::msg::Pose pose_params_to_pose(const PoseParams & pose_params) +{ + geometry_msgs::msg::Pose pose; + pose.position.x = pose_params.x; + pose.position.y = pose_params.y; + pose.position.z = pose_params.z; + + tf2::Quaternion pose_q_orientation; + pose_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(pose_params.roll), + tier4_autoware_utils::deg2rad(pose_params.pitch), + tier4_autoware_utils::deg2rad(pose_params.yaw)); + pose.orientation = tf2::toMsg(pose_q_orientation); + return pose; +} + +PointCloud2::SharedPtr create_entity_pointcloud_ptr( + const EntityParams & entity_params, const double pointcloud_sampling_distance) +{ + pcl::PointCloud point_cloud; + tf2::Quaternion entity_q_orientation; + + entity_q_orientation.setRPY( + tier4_autoware_utils::deg2rad(entity_params.roll), + tier4_autoware_utils::deg2rad(entity_params.pitch), + tier4_autoware_utils::deg2rad(entity_params.yaw)); + tf2::Transform tf(entity_q_orientation); + const auto origin = tf2::Vector3(entity_params.x, entity_params.y, entity_params.z); + tf.setOrigin(origin); + + const double it_x = entity_params.x_l / pointcloud_sampling_distance; + const double it_y = entity_params.y_l / pointcloud_sampling_distance; + const double it_z = entity_params.z_l / pointcloud_sampling_distance; + + // Sample the box and rotate + for (int i = 0; i <= it_z; ++i) { + for (int j = 0; j <= it_y; ++j) { + for (int k = 0; k <= it_x; ++k) { + const double p_x = -entity_params.x_l / 2 + k * pointcloud_sampling_distance; + const double p_y = -entity_params.y_l / 2 + j * pointcloud_sampling_distance; + const double p_z = -entity_params.z_l / 2 + i * pointcloud_sampling_distance; + const auto tmp = tf2::Vector3(p_x, p_y, p_z); + tf2::Vector3 data_out = tf * tmp; + point_cloud.emplace_back(pcl::PointXYZ( + static_cast(data_out.x()), static_cast(data_out.y()), + static_cast(data_out.z()))); + } + } + } + PointCloud2::SharedPtr entity_pointcloud_ptr; + entity_pointcloud_ptr = std::make_shared(); + pcl::toROSMsg(point_cloud, *entity_pointcloud_ptr); + return entity_pointcloud_ptr; +} + +PredictedObjects::SharedPtr create_entity_predicted_objects_ptr(const EntityParams & entity_params) +{ + unique_identifier_msgs::msg::UUID uuid_msg; + + PredictedObject obj; + const auto entity_pose = create_entity_pose(entity_params); + geometry_msgs::msg::Vector3 dimension; + dimension.set__x(entity_params.x_l); + dimension.set__y(entity_params.y_l); + dimension.set__z(entity_params.z_l); + obj.shape.set__dimensions(dimension); + + obj.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + obj.existence_probability = 1.0; + obj.kinematics.initial_pose_with_covariance.pose = entity_pose; + + autoware_auto_perception_msgs::msg::PredictedPath path; + path.confidence = 1.0; + path.path.emplace_back(entity_pose); + obj.kinematics.predicted_paths.emplace_back(path); + + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::CAR; + classification.probability = 1.0; + obj.classification.emplace_back(classification); + obj.set__object_id(generate_uuid_msg("test_obstacle")); + + PredictedObjects pred_objects; + pred_objects.objects.emplace_back(obj); + return std::make_shared(pred_objects); +} + +double calculate_entity_search_radius(const EntityParams & entity_params) +{ + return std::sqrt( + std::pow(entity_params.x_l, 2) + std::pow(entity_params.y_l, 2) + + std::pow(entity_params.z_l, 2)) / + 2.0; +} + +bool search_pointcloud_near_pose( + const pcl::PointCloud & pcl_pointcloud, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + pcl_pointcloud.points.begin(), pcl_pointcloud.points.end(), + [pose, search_radius](const auto & point) { + return tier4_autoware_utils::calcDistance3d(pose.position, point) <= search_radius; + }); +} + +bool search_predicted_objects_near_pose( + const PredictedObjects & predicted_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [pose, search_radius](const PredictedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.initial_pose_with_covariance.pose.position) <= + search_radius; + }); + ; +} + +bool search_detected_objects_near_pose( + const DetectedObjects & detected_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + detected_objects.objects.begin(), detected_objects.objects.end(), + [pose, search_radius](const DetectedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +bool search_tracked_objects_near_pose( + const TrackedObjects & tracked_objects, const geometry_msgs::msg::Pose & pose, + const double search_radius) +{ + return std::any_of( + tracked_objects.objects.begin(), tracked_objects.objects.end(), + [pose, search_radius](const TrackedObject & object) { + return tier4_autoware_utils::calcDistance3d( + pose.position, object.kinematics.pose_with_covariance.pose.position) <= + search_radius; + }); +} + +LatencyStats calculate_statistics(const std::vector & latency_vec) +{ + LatencyStats stats{0.0, 0.0, 0.0, 0.0, 0.0}; + stats.max = *max_element(latency_vec.begin(), latency_vec.end()); + stats.min = *min_element(latency_vec.begin(), latency_vec.end()); + + const double sum = std::accumulate(latency_vec.begin(), latency_vec.end(), 0.0); + stats.mean = sum / static_cast(latency_vec.size()); + + std::vector sorted_latencies = latency_vec; + std::sort(sorted_latencies.begin(), sorted_latencies.end()); + stats.median = sorted_latencies.size() % 2 == 0 + ? (sorted_latencies[sorted_latencies.size() / 2 - 1] + + sorted_latencies[sorted_latencies.size() / 2]) / + 2 + : sorted_latencies[sorted_latencies.size() / 2]; + + const double sq_sum = + std::inner_product(latency_vec.begin(), latency_vec.end(), latency_vec.begin(), 0.0); + stats.std_dev = + std::sqrt(sq_sum / static_cast(latency_vec.size()) - stats.mean * stats.mean); + return stats; +} + +void write_results( + rclcpp::Node * node, const std::string & output_file_path, const RunningMode & node_running_mode, + const std::vector & pipeline_map_vector) +{ + // create csv file + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + + std::stringstream ss; + ss << output_file_path; + if (!output_file_path.empty() && output_file_path.back() != '/') { + ss << "/"; // Ensure the path ends with a slash + } + if (node_running_mode == RunningMode::PlanningControl) { + ss << "planning_control-"; + } else { + ss << "perception_planning-"; + } + + ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d-%H-%M-%S"); + ss << "-reaction-results.csv"; + + // open file + std::ofstream file(ss.str()); + + // Check if the file was opened successfully + if (!file.is_open()) { + RCLCPP_ERROR_ONCE(node->get_logger(), "Failed to open file: %s", ss.str().c_str()); + return; + } + + // tmp map to store latency results for statistics + std::map>> tmp_latency_map; + + size_t test_count = 0; + for (const auto & pipeline_map : pipeline_map_vector) { + test_count++; + // convert pipeline_map to vector of tuples + file << "Test " << test_count << "\n"; + const auto sorted_results_vector = convert_pipeline_map_to_sorted_vector(pipeline_map); + const auto spawn_cmd_time = std::get<0>(*sorted_results_vector.begin()); + + for (size_t i = 0; i < sorted_results_vector.size(); ++i) { + const auto & [pipeline_header_time, pipeline_reactions] = sorted_results_vector[i]; + + // total time pipeline lasts + file << "Pipeline - " << i << ","; + + // pipeline nodes + for (const auto & [node_name, reaction] : pipeline_reactions) { + file << node_name << ","; + } + + file << "\nNode - Pipeline - Total Latency [ms],"; + + for (size_t j = 0; j < pipeline_reactions.size(); ++j) { + const auto & reaction = pipeline_reactions[j].second; + const auto & node_name = pipeline_reactions[j].first; + if (j == 0) { + const auto node_latency = + calculate_time_diff_ms(reaction.header.stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + const auto total_latency = + calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } else { + const auto & prev_reaction = pipeline_reactions[j - 1].second; + const auto node_latency = + calculate_time_diff_ms(prev_reaction.published_stamp, reaction.published_stamp); + const auto pipeline_latency = + calculate_time_diff_ms(pipeline_header_time, reaction.published_stamp); + const auto total_latency = + calculate_time_diff_ms(spawn_cmd_time, reaction.published_stamp); + file << node_latency << " - " << pipeline_latency << " - " << total_latency << ","; + tmp_latency_map[node_name].emplace_back( + std::make_tuple(node_latency, pipeline_latency, total_latency)); + } + } + file << "\n"; + } + } + + // write statistics + + file << "\nStatistics\n"; + file << "Node " + "Name,Min-NL,Max-NL,Mean-NL,Median-NL,Std-Dev-NL,Min-PL,Max-PL,Mean-PL,Median-PL,Std-Dev-" + "PL,Min-TL,Max-TL,Mean-TL,Median-TL,Std-Dev-TL\n"; + for (const auto & [node_name, latency_vec] : tmp_latency_map) { + file << node_name << ","; + + std::vector node_latencies; + std::vector pipeline_latencies; + std::vector total_latencies; + + // Extract latencies + for (const auto & latencies : latency_vec) { + double node_latency, pipeline_latency, total_latency; + std::tie(node_latency, pipeline_latency, total_latency) = latencies; + node_latencies.push_back(node_latency); + pipeline_latencies.push_back(pipeline_latency); + total_latencies.push_back(total_latency); + } + + const auto stats_node_latency = calculate_statistics(node_latencies); + const auto stats_pipeline_latency = calculate_statistics(pipeline_latencies); + const auto stats_total_latency = calculate_statistics(total_latencies); + + file << stats_node_latency.min << "," << stats_node_latency.max << "," + << stats_node_latency.mean << "," << stats_node_latency.median << "," + << stats_node_latency.std_dev << "," << stats_pipeline_latency.min << "," + << stats_pipeline_latency.max << "," << stats_pipeline_latency.mean << "," + << stats_pipeline_latency.median << "," << stats_pipeline_latency.std_dev << "," + << stats_total_latency.min << "," << stats_total_latency.max << "," + << stats_total_latency.mean << "," << stats_total_latency.median << "," + << stats_total_latency.std_dev << "\n"; + } + file.close(); + RCLCPP_INFO(node->get_logger(), "Results written to: %s", ss.str().c_str()); +} +} // namespace reaction_analyzer