From d71c8f6a431c33aef195618819c41e7ac03770aa Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 3 Dec 2024 14:55:04 +0900 Subject: [PATCH 1/2] test(blind_spot): add unit tests Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 16 +- .../manager.hpp | 3 +- .../parameter.hpp | 40 + .../scene.hpp | 15 +- .../util.hpp | 32 +- .../package.xml | 1 + .../src/manager.cpp | 18 +- .../src/parameter.cpp | 45 + .../src/scene.cpp | 24 +- .../src/util.cpp | 49 +- .../test/test_util.cpp | 446 +++ .../test_data/object_on_adj_lane.yaml | 2698 +++++++++++++++++ .../test_data/object_on_shoulder.yaml | 2512 +++++++++++++++ 13 files changed, 5822 insertions(+), 77 deletions(-) create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml create mode 100644 planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt index b28e486c9be05..220c380eb0fd7 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.14) project(autoware_behavior_velocity_blind_spot_module) +option(EXPORT_TEST_PLOT_FIGURE "Export plot figures in test" OFF) + find_package(autoware_cmake REQUIRED) autoware_package() pluginlib_export_plugin_description_file(autoware_behavior_velocity_planner plugins.xml) @@ -11,16 +13,20 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/scene.cpp src/decisions.cpp src/util.cpp + src/parameter.cpp ) if(BUILD_TESTING) + if(EXPORT_TEST_PLOT_FIGURE) + add_definitions(-DEXPORT_TEST_PLOT_FIGURE "-Wno-attributes") # // cspell: ignore DEXPORT + endif() find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - file(GLOB_RECURSE TEST_SOURCES test/*.cpp) - ament_add_ros_isolated_gtest(test_${PROJECT_NAME} - ${TEST_SOURCES} + # NOTE(soblin): pybind11::scoped_interpreter needs to be initialized globally, not in the FixtureClass instantiated for each test suite + ament_add_gtest(test_${PROJECT_NAME}_util + test/test_util.cpp ) - target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) + target_link_libraries(test_${PROJECT_NAME}_util ${PROJECT_NAME}) endif() -ament_auto_package(INSTALL_TO_SHARE config) +ament_auto_package(INSTALL_TO_SHARE config test_data) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp index bf01986d5c28f..41330e459a524 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/manager.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__MANAGER_HPP_ +#include "autoware/behavior_velocity_blind_spot_module/parameter.hpp" #include "autoware/behavior_velocity_blind_spot_module/scene.hpp" #include @@ -38,7 +39,7 @@ class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC const char * getModuleName() override { return "blind_spot"; } private: - BlindSpotModule::PlannerParam planner_param_; + PlannerParam planner_param_; void launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) override; diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp new file mode 100644 index 0000000000000..59322caecb62f --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/parameter.hpp @@ -0,0 +1,40 @@ +// 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__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ +#define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ +struct PlannerParam +{ + static PlannerParam init(rclcpp::Node & node, const std::string & ns); + bool use_pass_judge_line{}; + double stop_line_margin{}; + double backward_detection_length{}; + double ignore_width_from_center_line{}; + double adjacent_extend_width{}; + double opposite_adjacent_extend_width{}; + double max_future_movement_time{}; + double ttc_min{}; + double ttc_max{}; + double ttc_ego_minimal_velocity{}; +}; +} // namespace autoware::behavior_velocity_planner + +#endif // AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__PARAMETER_HPP_ diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp index 50ce8634ec600..5e8ef9fc8a063 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/scene.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__SCENE_HPP_ +#include #include #include #include @@ -70,20 +71,6 @@ class BlindSpotModule : public SceneModuleInterfaceWithRTC }; public: - struct PlannerParam - { - bool use_pass_judge_line; - double stop_line_margin; - double backward_detection_length; - double ignore_width_from_center_line; - double adjacent_extend_width; - double opposite_adjacent_extend_width; - double max_future_movement_time; - double ttc_min; - double ttc_max; - double ttc_ego_minimal_velocity; - }; - BlindSpotModule( const int64_t module_id, const int64_t lane_id, const TurnDirection turn_direction, const std::shared_ptr planner_data, const PlannerParam & planner_param, diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp index 9d908414b6d95..e18d96709ef92 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/include/autoware/behavior_velocity_blind_spot_module/util.hpp @@ -16,6 +16,7 @@ #define AUTOWARE__BEHAVIOR_VELOCITY_BLIND_SPOT_MODULE__UTIL_HPP_ #include +#include #include @@ -25,6 +26,7 @@ #include #include #include +#include namespace autoware::behavior_velocity_planner { @@ -50,31 +52,49 @@ std::optional generateInterpolatedPathInfo( const lanelet::Id lane_id, const tier4_planning_msgs::msg::PathWithLaneId & input_path, rclcpp::Logger logger); +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length); + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr); /** - * @brief Create half lanelet - * @param lanelet input lanelet - * @return Half lanelet + * @brief generate a new lanelet object on the `turn_direction` side of `lanelet` which is offset + * from `ignore_width_from_centerline` from the centerline of `lanelet` + * @return new lanelet object */ lanelet::ConstLanelet generateHalfLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection & turn_direction, const double ignore_width_from_centerline); +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring lanelet of the + * input `lanelet` by the width of `adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double adjacent_extend_width); + +/** + * @brief generate a new lanelet object from the `turn_direction` side neighboring opposite lanelet + * of the input `lanelet` by the width of `opposite_adjacent_extend_width` + * @param new lanelet object + */ lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction, const double opposite_adjacent_extend_width); +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id); + lanelet::ConstLanelets generateBlindSpotLanelets( const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width); + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width); /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml index 8c34678f439ed..dabd3045b31d2 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/package.xml @@ -25,6 +25,7 @@ autoware_perception_msgs autoware_planning_msgs autoware_route_handler + autoware_test_utils autoware_universe_utils geometry_msgs pluginlib diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp index a1410d2fecfd5..d6cae9004600a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/manager.cpp @@ -34,23 +34,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) node, getModuleName(), getEnableRTC(node, std::string(getModuleName()) + ".enable_rtc")) { const std::string ns(BlindSpotModuleManager::getModuleName()); - planner_param_.use_pass_judge_line = - getOrDeclareParameter(node, ns + ".use_pass_judge_line"); - planner_param_.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); - planner_param_.backward_detection_length = - getOrDeclareParameter(node, ns + ".backward_detection_length"); - planner_param_.ignore_width_from_center_line = - getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); - planner_param_.adjacent_extend_width = - getOrDeclareParameter(node, ns + ".adjacent_extend_width"); - planner_param_.opposite_adjacent_extend_width = - getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); - planner_param_.max_future_movement_time = - getOrDeclareParameter(node, ns + ".max_future_movement_time"); - planner_param_.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); - planner_param_.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); - planner_param_.ttc_ego_minimal_velocity = - getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + planner_param_ = PlannerParam::init(node, ns); } void BlindSpotModuleManager::launchNewModules(const tier4_planning_msgs::msg::PathWithLaneId & path) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp new file mode 100644 index 0000000000000..0984de6f73262 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/parameter.cpp @@ -0,0 +1,45 @@ +// 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 "autoware/behavior_velocity_blind_spot_module/parameter.hpp" + +#include + +#include + +namespace autoware::behavior_velocity_planner +{ + +PlannerParam PlannerParam::init(rclcpp::Node & node, const std::string & ns) +{ + using autoware::universe_utils::getOrDeclareParameter; + PlannerParam param; + param.use_pass_judge_line = getOrDeclareParameter(node, ns + ".use_pass_judge_line"); + param.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + param.backward_detection_length = + getOrDeclareParameter(node, ns + ".backward_detection_length"); + param.ignore_width_from_center_line = + getOrDeclareParameter(node, ns + ".ignore_width_from_center_line"); + param.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + param.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); + param.max_future_movement_time = + getOrDeclareParameter(node, ns + ".max_future_movement_time"); + param.ttc_min = getOrDeclareParameter(node, ns + ".ttc_min"); + param.ttc_max = getOrDeclareParameter(node, ns + ".ttc_max"); + param.ttc_ego_minimal_velocity = + getOrDeclareParameter(node, ns + ".ttc_ego_minimal_velocity"); + return param; +} +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp index 7d1a68c5006a0..8a9401646aaea 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/scene.cpp @@ -101,8 +101,9 @@ BlindSpotDecision BlindSpotModule::modifyPathVelocityDetail(PathWithLaneId * pat } if (!blind_spot_lanelets_) { + const auto lane_ids_upto_intersection = find_lane_ids_upto(input_path, lane_id_); const auto blind_spot_lanelets = generateBlindSpotLanelets( - planner_data_->route_handler_, turn_direction_, lane_id_, input_path, + planner_data_->route_handler_, turn_direction_, lane_ids_upto_intersection, planner_param_.ignore_width_from_center_line, planner_param_.adjacent_extend_width, planner_param_.opposite_adjacent_extend_width); if (!blind_spot_lanelets.empty()) { @@ -179,27 +180,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path) return true; } -static std::optional getFirstPointIntersectsLineByFootprint( - const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, - const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) -{ - const auto & path_ip = interpolated_path_info.path; - const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); - const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); - const size_t start = - static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); - const auto line2d = line.basicLineString(); - for (auto i = start; i <= lane_end; ++i) { - const auto & base_pose = path_ip.points.at(i).point.pose; - const auto path_footprint = autoware::universe_utils::transformVector( - footprint, autoware::universe_utils::pose2transform(base_pose)); - if (bg::intersects(path_footprint, line2d)) { - return std::make_optional(i); - } - } - return std::nullopt; -} - static std::optional getDuplicatedPointIdx( const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & point) { diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp index 5c5aa0a26b3b4..8451661b2b71f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/src/util.cpp @@ -88,6 +88,27 @@ std::optional generateInterpolatedPathInfo( return interpolated_path_info; } +std::optional getFirstPointIntersectsLineByFootprint( + const lanelet::ConstLineString2d & line, const InterpolatedPathInfo & interpolated_path_info, + const autoware::universe_utils::LinearRing2d & footprint, const double vehicle_length) +{ + const auto & path_ip = interpolated_path_info.path; + const auto [lane_start, lane_end] = interpolated_path_info.lane_id_interval.value(); + const size_t vehicle_length_idx = static_cast(vehicle_length / interpolated_path_info.ds); + const size_t start = + static_cast(std::max(0, static_cast(lane_start) - vehicle_length_idx)); + const auto line2d = line.basicLineString(); + for (auto i = start; i <= lane_end; ++i) { + const auto & base_pose = path_ip.points.at(i).point.pose; + const auto path_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(base_pose)); + if (boost::geometry::intersects(path_footprint, line2d)) { + return std::make_optional(i); + } + } + return std::nullopt; +} + std::optional getSiblingStraightLanelet( const lanelet::Lanelet assigned_lane, const lanelet::routing::RoutingGraphConstPtr routing_graph_ptr) @@ -200,15 +221,9 @@ static lanelet::LineString3d removeConst(lanelet::ConstLineString3d line) return lanelet::LineString3d(lanelet::InvalId, pts); } -lanelet::ConstLanelets generateBlindSpotLanelets( - const std::shared_ptr route_handler, - const TurnDirection turn_direction, const lanelet::Id lane_id, - const tier4_planning_msgs::msg::PathWithLaneId & path, const double ignore_width_from_centerline, - const double adjacent_extend_width, const double opposite_adjacent_extend_width) +std::vector find_lane_ids_upto( + const tier4_planning_msgs::msg::PathWithLaneId & path, const lanelet::Id lane_id) { - const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); - const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); - std::vector lane_ids; /* get lane ids until intersection */ for (const auto & point : path.points) { @@ -216,19 +231,29 @@ lanelet::ConstLanelets generateBlindSpotLanelets( for (const auto id : point.lane_ids) { if (id == lane_id) { found_intersection_lane = true; - lane_ids.push_back(lane_id); break; } // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); + if (std::find(lane_ids.begin(), lane_ids.end(), id) == lane_ids.end()) { + lane_ids.push_back(id); } } if (found_intersection_lane) break; } + return lane_ids; +} + +lanelet::ConstLanelets generateBlindSpotLanelets( + const std::shared_ptr route_handler, + const TurnDirection turn_direction, const std::vector & lane_ids_upto_intersection, + const double ignore_width_from_centerline, const double adjacent_extend_width, + const double opposite_adjacent_extend_width) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + const auto routing_graph_ptr = route_handler->getRoutingGraphPtr(); lanelet::ConstLanelets blind_spot_lanelets; - for (const auto i : lane_ids) { + for (const auto i : lane_ids_upto_intersection) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); const auto ego_half_lanelet = generateHalfLanelet(lane, turn_direction, ignore_width_from_centerline); diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp new file mode 100644 index 0000000000000..2586019797ddd --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -0,0 +1,446 @@ +// 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 +#include +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#ifdef EXPORT_TEST_PLOT_FIGURE +#include +#include +#include +#include + +#include +#include // needed for passing std::string to Args + +#endif + +using autoware::test_utils::parse; + +class TestWithAdjLaneData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_adj_lane.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{2200}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithAdjLaneData, getSiblingStraightLanelet) +{ + const auto sibling_straight_lanelet_opt = + autoware::behavior_velocity_planner::getSiblingStraightLanelet( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), + route_handler->getRoutingGraphPtr()); + ASSERT_NO_FATAL_FAILURE({ ASSERT_TRUE(sibling_straight_lanelet_opt.has_value()); }); + EXPECT_EQ(sibling_straight_lanelet_opt.value().id(), 2100); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(lane_id_), ax, + autoware::test_utils::LaneConfig{"intersection turning lanes", LineConfig{"blue"}}); + + // for illustration + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(3010933), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2201), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2202), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"blue"}}); + autoware::test_utils::plot_lanelet2_object( + route_handler->getLaneletMapPtr()->laneletLayer.get(2010), ax, + autoware::test_utils::LaneConfig{std::nullopt, LineConfig{"green"}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + autoware::test_utils::plot_lanelet2_object( + sibling_straight_lanelet_opt.value(), ax, + LaneConfig{"sibling_straight_lanelet", LineConfig{"red"}}); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateHalfLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + + const auto half_lanelet = autoware::behavior_velocity_planner::generateHalfLanelet( + lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.ignore_width_from_center_line); + + /* + TODO(soblin): how to check if they overlap only on the left line string + EXPECT_EQ( + boost::geometry::within( + half_lanelet.polygon2d().basicPolygon(), lanelet.polygon2d().basicPolygon()), + true); + */ + EXPECT_EQ( + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / 2.0 > + boost::geometry::area(half_lanelet.polygon2d().basicPolygon()), + true); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"original", LineConfig{"blue", 1.5}}); + autoware::test_utils::plot_lanelet2_object( + half_lanelet, ax, LaneConfig{"half lanelet", LineConfig{"red", 0.75}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + // const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, 650)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateExtendedAdjacentLanelet) +{ + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(2010); + const auto adj_lanelet = *(route_handler->getRoutingGraphPtr()->adjacentLeft(lanelet)); + + const auto extended_adj_lanelet = + autoware::behavior_velocity_planner::generateExtendedAdjacentLanelet( + adj_lanelet, autoware::behavior_velocity_planner::TurnDirection::LEFT, + param.adjacent_extend_width); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, autoware::test_utils::LaneConfig{"given", LineConfig{"blue", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + adj_lanelet, ax, autoware::test_utils::LaneConfig{"adjacent", LineConfig{"green", 1.0}}); + autoware::test_utils::plot_lanelet2_object( + extended_adj_lanelet, ax, LaneConfig{"generated", LineConfig{"red", 2.0}}); + + const auto [x0, x1] = ax.get_xlim(); + const auto [y0, y1] = ax.get_ylim(); + const double width = x1 - x0; + const double height = y1 - y0; + ax.set_xlim(Args(x0, x0 + width * 1.3)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {2000, 2010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + const auto [y0, y1] = ax.get_ylim(); + const double height = y1 - y0; + ax.set_xlim(Args(300, 365)); + ax.set_ylim(Args(y0, y0 + height * 1.3)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateBlindSpotLanelets_right) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::RIGHT, {3008067}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 1); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {3008057, 3008054, 3008056, 3008061, 3008062, 3008059, 3008067, 3008066}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(905, 920)); + ax.set_ylim(Args(650, 950)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +TEST_F(TestWithAdjLaneData, generateInterpolatedPathInfo) +{ + const auto interpolated_path_info_opt = + autoware::behavior_velocity_planner::generateInterpolatedPathInfo( + lane_id_, path_with_lane_id, rclcpp::get_logger("test")); + EXPECT_EQ(interpolated_path_info_opt.has_value(), true); + const auto & interpolated_path_info = interpolated_path_info_opt.value(); + EXPECT_EQ(interpolated_path_info.lane_id_interval.has_value(), true); + const auto [start, end] = interpolated_path_info.lane_id_interval.value(); + tier4_planning_msgs::msg::PathWithLaneId interpolated_path; + for (auto i = start; i <= end; ++i) { + interpolated_path.points.push_back(interpolated_path_info.path.points.at(i)); + } +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + using autoware::test_utils::PathWithLaneIdConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {2010, 3010933, 2200, 3010920, 3010933, 2000, 3500}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + autoware::test_utils::plot_autoware_object( + path_with_lane_id, ax, PathWithLaneIdConfig{"original path", "red", 0.75}); + autoware::test_utils::plot_autoware_object( + interpolated_path, ax, PathWithLaneIdConfig{"interpolated path on lane 2010", "blue", 1.0}); + const auto [x0, x1] = ax.get_xlim(); + ax.set_xlim(Args(335, x1)); + ax.set_ylim(Args(640, 670)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper right"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +class TestWithShoulderData : public ::testing::Test +{ +protected: + void SetUp() override + { + const auto test_data_file = + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/object_on_shoulder.yaml"; + const auto config = YAML::LoadFile(test_data_file); + const auto route = parse(config["route"]); + const auto map_path = + autoware::test_utils::resolve_pkg_share_uri(config["map_path_uri"].as()); + if (!map_path) { + ASSERT_DEATH({ assert(false); }, "invalid map path"); + } + const auto intersection_map = autoware::test_utils::make_map_bin_msg(map_path.value()); + route_handler = std::make_shared(); + route_handler->setMap(intersection_map); + route_handler->setRoute(route); + self_odometry = autoware::test_utils::create_const_shared_ptr( + parse(config["self_odometry"])); + dynamic_object = autoware::test_utils::create_const_shared_ptr( + parse(config["dynamic_object"])); + path_with_lane_id = + parse(config["path_with_lane_id"]); + + // parameter + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", + "--params-file", + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/config/blind_spot.param.yaml", + }); + + auto node = rclcpp::Node::make_shared("blind_spot_test", node_options); + param = autoware::behavior_velocity_planner::PlannerParam::init(*node, "blind_spot"); + } + + std::shared_ptr route_handler{}; + std::shared_ptr self_odometry{}; + std::shared_ptr dynamic_object{}; + const lanelet::Id lane_id_{1010}; + tier4_planning_msgs::msg::PathWithLaneId path_with_lane_id; + autoware::behavior_velocity_planner::PlannerParam param; +}; + +TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) +{ + const auto blind_spot_lanelets = autoware::behavior_velocity_planner::generateBlindSpotLanelets( + route_handler, autoware::behavior_velocity_planner::TurnDirection::LEFT, {1000, 1010}, + param.ignore_width_from_center_line, param.adjacent_extend_width, + param.opposite_adjacent_extend_width); + EXPECT_EQ(blind_spot_lanelets.size(), 2); + +#ifdef EXPORT_TEST_PLOT_FIGURE + py::gil_scoped_acquire acquire; + using autoware::test_utils::LaneConfig; + using autoware::test_utils::LineConfig; + auto plt = autoware::pyplot::import(); + auto [fig, axes] = plt.subplots(1, 1); + auto & ax = axes[0]; + for (const auto & id : {1000, 1010, 3010907, 3010879, 1200, 1100}) { + const auto lanelet = route_handler->getLaneletMapPtr()->laneletLayer.get(id); + autoware::test_utils::plot_lanelet2_object( + lanelet, ax, LaneConfig{std::nullopt, LineConfig{"k", 0.75}}); + } + for (const auto & blind_spot_lanelet : blind_spot_lanelets) { + autoware::test_utils::plot_lanelet2_object( + blind_spot_lanelet, ax, LaneConfig{"blind_spot_lanelet", LineConfig{"blue", 2.0}}); + } + ax.set_xlim(Args(330, 365)); + ax.set_ylim(Args(580, 625)); + ax.set_aspect(Args("equal")); + ax.grid(); + ax.legend(Args(), Kwargs{"loc"_a = "upper left"}); + fig.tight_layout(); + const std::string filename = std::string( + ament_index_cpp::get_package_share_directory("autoware_behavior_velocity_blind_spot_module") + + "/test_data/" + ::testing::UnitTest::GetInstance()->current_test_info()->name() + ".svg"); + plt.savefig(Args(filename)); +#endif +} + +int main(int argc, char ** argv) +{ + py::scoped_interpreter guard{}; + rclcpp::init(0, nullptr); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml new file mode 100644 index 0000000000000..97068d69cd296 --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_adj_lane.yaml @@ -0,0 +1,2698 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 563 + nanosec: 125840339 + frame_id: map + points: + - point: + pose: + position: + x: 330.010 + y: 643.206 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00655991 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 332.010 + y: 643.233 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656125 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 334.009 + y: 643.259 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658745 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 336.009 + y: 643.285 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00660718 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 338.009 + y: 643.312 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00658719 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 340.009 + y: 643.338 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00656661 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.009 + y: 643.364 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00657433 + w: 0.999978 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - point: + pose: + position: + x: 342.809 + y: 643.375 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0162350 + w: 0.999868 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2010 + - 2200 + - point: + pose: + position: + x: 344.008 + y: 643.414 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0355542 + w: 0.999368 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 346.003 + y: 643.556 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0600763 + w: 0.998194 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 347.986 + y: 643.796 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.134724 + w: 0.990883 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 349.912 + y: 644.329 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.199426 + w: 0.979913 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 351.776 + y: 645.121 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.281519 + w: 0.959556 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 353.448 + y: 646.194 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.345347 + w: 0.938475 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 354.997 + y: 647.513 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.407454 + w: 0.913226 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 356.322 + y: 648.989 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.480104 + w: 0.877212 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 357.418 + y: 650.701 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.544895 + w: 0.838504 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.228 + y: 652.525 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.610262 + w: 0.792199 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 358.741 + y: 654.467 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.646506 + w: 0.762909 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.070 + y: 656.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697457 + w: 0.716627 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.124 + y: 658.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.713869 + w: 0.700280 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.085 + y: 660.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714886 + w: 0.699241 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - point: + pose: + position: + x: 359.052 + y: 661.935 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2200 + - 3500 + - point: + pose: + position: + x: 359.040 + y: 662.438 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.993 + y: 664.437 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.945 + y: 666.436 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715334 + w: 0.698782 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.900 + y: 668.374 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714518 + w: 0.699617 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.858 + y: 670.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712894 + w: 0.701272 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.825 + y: 672.373 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710606 + w: 0.703590 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.807 + y: 674.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + - point: + pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 3500 + left_bound: + - x: 329.488 + y: 644.822 + z: 100.000 + - x: 332.672 + y: 644.864 + z: 100.000 + - x: 334.352 + y: 644.886 + z: 100.000 + - x: 336.351 + y: 644.913 + z: 100.000 + - x: 337.719 + y: 644.931 + z: 100.000 + - x: 340.352 + y: 644.965 + z: 100.000 + - x: 342.813 + y: 645.020 + z: 100.000 + - x: 344.277 + y: 645.102 + z: 100.000 + - x: 346.200 + y: 645.256 + z: 100.000 + - x: 347.374 + y: 645.428 + z: 100.000 + - x: 348.470 + y: 645.697 + z: 100.000 + - x: 349.568 + y: 646.035 + z: 100.000 + - x: 350.592 + y: 646.513 + z: 100.000 + - x: 351.572 + y: 647.043 + z: 100.000 + - x: 352.507 + y: 647.713 + z: 100.000 + - x: 353.327 + y: 648.476 + z: 100.000 + - x: 354.109 + y: 649.196 + z: 100.000 + - x: 354.661 + y: 649.830 + z: 100.000 + - x: 355.275 + y: 650.523 + z: 100.000 + - x: 356.177 + y: 652.020 + z: 100.000 + - x: 356.615 + y: 653.084 + z: 100.000 + - x: 356.929 + y: 654.139 + z: 100.000 + - x: 357.146 + y: 655.241 + z: 100.000 + - x: 357.284 + y: 656.160 + z: 100.000 + - x: 357.312 + y: 657.420 + z: 100.000 + - x: 357.425 + y: 658.703 + z: 100.000 + - x: 357.383 + y: 660.465 + z: 100.000 + - x: 357.349 + y: 661.894 + z: 100.000 + - x: 357.386 + y: 664.700 + z: 100.000 + - x: 357.338 + y: 666.699 + z: 100.000 + - x: 357.291 + y: 668.699 + z: 100.000 + - x: 357.243 + y: 670.698 + z: 100.000 + - x: 357.195 + y: 672.698 + z: 100.000 + - x: 357.148 + y: 674.697 + z: 100.000 + - x: 357.021 + y: 680.023 + z: 100.000 + right_bound: + - x: 329.531 + y: 641.578 + z: 100.000 + - x: 332.717 + y: 641.619 + z: 100.000 + - x: 334.395 + y: 641.641 + z: 100.000 + - x: 336.394 + y: 641.668 + z: 100.000 + - x: 337.761 + y: 641.686 + z: 100.000 + - x: 340.394 + y: 641.721 + z: 100.000 + - x: 342.805 + y: 641.730 + z: 100.000 + - x: 344.462 + y: 641.823 + z: 100.000 + - x: 346.283 + y: 641.878 + z: 100.000 + - x: 347.708 + y: 641.985 + z: 100.000 + - x: 349.158 + y: 642.284 + z: 100.000 + - x: 350.570 + y: 642.681 + z: 100.000 + - x: 351.954 + y: 643.219 + z: 100.000 + - x: 353.269 + y: 643.933 + z: 100.000 + - x: 354.499 + y: 644.727 + z: 100.000 + - x: 355.646 + y: 645.678 + z: 100.000 + - x: 356.516 + y: 646.483 + z: 100.000 + - x: 357.282 + y: 647.228 + z: 100.000 + - x: 357.690 + y: 647.819 + z: 100.000 + - x: 358.527 + y: 649.081 + z: 100.000 + - x: 359.237 + y: 650.418 + z: 100.000 + - x: 359.789 + y: 651.774 + z: 100.000 + - x: 360.225 + y: 653.220 + z: 100.000 + - x: 360.523 + y: 654.391 + z: 100.000 + - x: 360.701 + y: 655.780 + z: 100.000 + - x: 360.976 + y: 657.487 + z: 100.000 + - x: 360.804 + y: 658.842 + z: 100.000 + - x: 360.774 + y: 660.766 + z: 100.000 + - x: 360.755 + y: 661.975 + z: 100.000 + - x: 360.585 + y: 664.776 + z: 100.000 + - x: 360.537 + y: 666.776 + z: 100.000 + - x: 360.490 + y: 668.775 + z: 100.000 + - x: 360.442 + y: 670.774 + z: 100.000 + - x: 360.394 + y: 672.774 + z: 100.000 + - x: 360.347 + y: 674.773 + z: 100.000 + - x: 360.220 + y: 680.099 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 563 + nanosec: 160706650 + frame_id: map + objects: + - object_id: + uuid: + - 6 + - 144 + - 167 + - 221 + - 26 + - 196 + - 145 + - 186 + - 207 + - 95 + - 21 + - 211 + - 96 + - 13 + - 92 + - 5 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + covariance: + - 0.415785 + - -0.000621846 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000621846 + - 0.388031 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0108039 + initial_twist_with_covariance: + twist: + linear: + x: 9.96635 + y: 3.24754e-05 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 5.69744e-05 + covariance: + - 1.01780 + - -2.02150e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -3.54650e-05 + - -2.02150e-05 + - 0.00403854 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -3.54650e-05 + - 0.00708516 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0124301 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 404.688 + y: 647.065 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 409.670 + y: 647.155 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 414.653 + y: 647.245 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 419.635 + y: 647.335 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 424.617 + y: 647.425 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 429.600 + y: 647.515 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 434.582 + y: 647.605 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 439.565 + y: 647.694 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 444.547 + y: 647.784 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 449.529 + y: 647.874 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 454.512 + y: 647.964 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 459.494 + y: 648.054 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 464.476 + y: 648.144 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 469.459 + y: 648.233 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 474.441 + y: 648.323 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 479.423 + y: 648.413 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 484.406 + y: 648.503 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 489.388 + y: 648.593 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 494.371 + y: 648.683 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 499.353 + y: 648.773 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + - position: + x: 504.335 + y: 648.862 + z: 100.630 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00901435 + w: 0.999959 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 1.00000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.501209 + z: 0.690476 + - object_id: + uuid: + - 32 + - 119 + - 141 + - 16 + - 72 + - 85 + - 32 + - 153 + - 66 + - 136 + - 88 + - 177 + - 247 + - 166 + - 151 + - 80 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + covariance: + - 0.0657687 + - -0.000366900 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000366900 + - 0.0451891 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00300423 + initial_twist_with_covariance: + twist: + linear: + x: 10.0048 + y: -0.00124906 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00219133 + covariance: + - 0.522271 + - -6.09993e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000107016 + - -6.09993e-05 + - 0.00263830 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000107016 + - 0.00462859 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00812034 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00453640 + w: 0.999990 + - position: + x: 333.933 + y: 644.723 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00622365 + w: 0.999981 + - position: + x: 338.935 + y: 644.640 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00828910 + w: 0.999966 + - position: + x: 343.877 + y: 644.600 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00409926 + w: 0.999992 + - position: + x: 348.624 + y: 645.122 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0547342 + w: 0.998501 + - position: + x: 352.898 + y: 646.993 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.204867 + w: 0.978790 + - position: + x: 356.221 + y: 650.176 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.372718 + w: 0.927945 + - position: + x: 358.029 + y: 654.521 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.554937 + w: 0.831892 + - position: + x: 358.484 + y: 659.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.673390 + w: 0.739287 + - position: + x: 358.380 + y: 664.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714418 + w: 0.699720 + - position: + x: 358.261 + y: 669.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 674.391 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 679.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.904 + y: 684.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 689.392 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.665 + y: 694.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.546 + y: 699.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.427 + y: 704.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.308 + y: 709.394 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.188 + y: 714.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.069 + y: 719.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.950 + y: 724.396 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.831 + y: 729.397 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.712 + y: 734.398 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.593 + y: 739.399 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.473 + y: 744.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.354 + y: 749.400 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.235 + y: 754.401 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.116 + y: 759.402 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.997 + y: 764.403 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.880 + y: 644.606 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00392303 + w: 0.999992 + - position: + x: 348.786 + y: 644.906 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0304731 + w: 0.999536 + - position: + x: 353.463 + y: 645.966 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.111241 + w: 0.993793 + - position: + x: 357.696 + y: 648.148 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.235723 + w: 0.971820 + - position: + x: 361.186 + y: 651.334 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.361617 + w: 0.932327 + - position: + x: 363.716 + y: 655.456 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.488304 + w: 0.872674 + - position: + x: 364.853 + y: 660.052 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.616332 + w: 0.787487 + - position: + x: 364.887 + y: 665.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704715 + w: 0.709490 + - position: + x: 364.768 + y: 670.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.648 + y: 675.003 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.529 + y: 680.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.410 + y: 685.004 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.291 + y: 690.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.172 + y: 695.005 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 364.053 + y: 700.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.933 + y: 705.006 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.814 + y: 710.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.695 + y: 715.007 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.576 + y: 720.008 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.457 + y: 725.009 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.338 + y: 730.010 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.218 + y: 735.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 363.099 + y: 740.011 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.980 + y: 745.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.861 + y: 750.013 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.742 + y: 755.014 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 362.622 + y: 760.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.786 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00451016 + w: 0.999990 + - position: + x: 333.933 + y: 644.725 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00607237 + w: 0.999982 + - position: + x: 338.935 + y: 644.645 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00796688 + w: 0.999968 + - position: + x: 343.873 + y: 644.613 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00329511 + w: 0.999995 + - position: + x: 348.698 + y: 645.015 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0415760 + w: 0.999135 + - position: + x: 353.223 + y: 646.395 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.147464 + w: 0.989067 + - position: + x: 357.115 + y: 649.075 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.296991 + w: 0.954880 + - position: + x: 359.997 + y: 652.871 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.444541 + w: 0.895759 + - position: + x: 361.525 + y: 657.337 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.581548 + w: 0.813512 + - position: + x: 361.651 + y: 662.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.697878 + w: 0.716217 + - position: + x: 361.533 + y: 667.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715386 + w: 0.698729 + - position: + x: 361.414 + y: 672.205 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.295 + y: 677.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.176 + y: 682.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 361.057 + y: 687.206 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.938 + y: 692.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.818 + y: 697.207 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.699 + y: 702.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.580 + y: 707.208 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.461 + y: 712.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.342 + y: 717.209 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.222 + y: 722.210 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.103 + y: 727.211 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.984 + y: 732.212 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.865 + y: 737.213 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.746 + y: 742.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.626 + y: 747.214 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.507 + y: 752.215 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.388 + y: 757.216 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.269 + y: 762.217 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + - path: + - position: + x: 323.928 + y: 644.831 + z: 100.568 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00410256 + w: 0.999992 + - position: + x: 328.930 + y: 644.785 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00457230 + w: 0.999990 + - position: + x: 333.933 + y: 644.721 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00644958 + w: 0.999979 + - position: + x: 338.935 + y: 644.632 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00883007 + w: 0.999961 + - position: + x: 343.938 + y: 644.526 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0105871 + w: 0.999944 + - position: + x: 348.941 + y: 644.416 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.0110042 + w: 0.999939 + - position: + x: 353.943 + y: 644.319 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00972075 + w: 0.999953 + - position: + x: 358.946 + y: 644.250 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00691289 + w: 0.999976 + - position: + x: 363.948 + y: 644.219 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.00309863 + w: 0.999995 + - position: + x: 368.950 + y: 644.228 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.000886733 + w: 1.00000 + - position: + x: 373.952 + y: 644.264 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00363761 + w: 0.999993 + - position: + x: 378.954 + y: 644.305 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00413557 + w: 0.999991 + - position: + x: 383.955 + y: 644.347 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00420318 + w: 0.999991 + - position: + x: 388.957 + y: 644.388 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00411113 + w: 0.999992 + - position: + x: 393.958 + y: 644.432 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00439488 + w: 0.999990 + - position: + x: 398.956 + y: 644.454 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00215849 + w: 0.999998 + - position: + x: 403.941 + y: 644.630 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0176845 + w: 0.999844 + - position: + x: 408.939 + y: 644.821 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 413.938 + y: 645.012 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 418.936 + y: 645.202 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 423.935 + y: 645.393 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 428.933 + y: 645.584 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 433.932 + y: 645.774 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 438.931 + y: 645.965 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 443.929 + y: 646.156 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 448.928 + y: 646.346 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 453.927 + y: 646.537 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 458.925 + y: 646.728 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 463.924 + y: 646.918 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 468.923 + y: 647.109 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + - position: + x: 473.922 + y: 647.300 + z: 100.384 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.0190608 + w: 0.999818 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.250000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.604998 + z: 0.767451 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 21 + nanosec: 17351334 + frame_id: map + start_pose: + position: + x: 301.475 + y: 642.905 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00500123 + w: 0.999987 + goal_pose: + position: + x: 358.812 + y: 675.174 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705159 + w: 0.709049 + segments: + - preferred_primitive: + id: 2000 + primitive_type: "" + primitives: + - id: 2000 + primitive_type: lane + - id: 2001 + primitive_type: lane + - id: 2002 + primitive_type: lane + - preferred_primitive: + id: 2010 + primitive_type: "" + primitives: + - id: 2010 + primitive_type: lane + - preferred_primitive: + id: 2200 + primitive_type: "" + primitives: + - id: 2200 + primitive_type: lane + - preferred_primitive: + id: 3500 + primitive_type: "" + primitives: + - id: 3500 + primitive_type: lane + - id: 3501 + primitive_type: lane + - id: 3502 + primitive_type: lane + uuid: + uuid: + - 195 + - 84 + - 241 + - 105 + - 133 + - 218 + - 136 + - 83 + - 104 + - 191 + - 48 + - 109 + - 85 + - 60 + - 177 + - 115 + allow_modification: false +operation_mode: + stamp: + sec: 550 + nanosec: 913829317 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: /base_link + accel: + accel: + linear: + x: -0.770147 + y: 0.0409905 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 563 + nanosec: 188542779 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 337.371 + y: 643.219 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.00401753 + w: 0.999992 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 4.13155 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00992133 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml new file mode 100644 index 0000000000000..9a20cbee9c1ff --- /dev/null +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test_data/object_on_shoulder.yaml @@ -0,0 +1,2512 @@ +# +# AUTO GENERATED by autoware_test_utils::topic_snapshot_saver +# format1: +# +# format_version: +# map_path_uri: package:/// +# fields(this is array) +# - name: +# type: either {Odometry | AccelWithCovarianceStamped | PredictedObjects | OperationModeState | LaneletRoute | TrafficLightGroupArray | TrackedObjects | PathWithLaneId | TBD} +# topic: +# +format_version: 1 +map_path_uri: package://autoware_test_utils/test_map/intersection/lanelet2_map.osm +path_with_lane_id: + header: + stamp: + sec: 74 + nanosec: 11949715 + frame_id: map + points: + - point: + pose: + position: + x: 360.306 + y: 597.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710800 + w: 0.703394 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.285 + y: 599.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710829 + w: 0.703365 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.264 + y: 601.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710857 + w: 0.703336 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.243 + y: 603.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710881 + w: 0.703312 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.222 + y: 605.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710951 + w: 0.703242 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.200 + y: 607.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711036 + w: 0.703156 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.177 + y: 609.246 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711601 + w: 0.702584 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.152 + y: 611.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711932 + w: 0.702248 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - point: + pose: + position: + x: 360.133 + y: 612.628 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716985 + w: 0.697089 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1010 + - 1200 + - point: + pose: + position: + x: 360.116 + y: 613.245 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.732064 + w: 0.681236 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.972 + y: 615.239 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.740537 + w: 0.672016 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.778 + y: 617.231 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.759018 + w: 0.651069 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.474 + y: 619.204 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.784627 + w: 0.619968 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 359.010 + y: 621.159 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.835622 + w: 0.549305 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 358.221 + y: 622.986 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.876871 + w: 0.480726 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 357.127 + y: 624.700 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.906015 + w: 0.423245 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 355.847 + y: 626.230 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.934815 + w: 0.355135 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 354.341 + y: 627.567 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.956562 + w: 0.291529 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 352.674 + y: 628.688 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.976933 + w: 0.213544 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 350.854 + y: 629.523 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.989029 + w: 0.147720 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 348.920 + y: 630.114 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.997418 + w: 0.0718077 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 346.951 + y: 630.399 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121684 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 344.951 + y: 630.448 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 1.00000 + w: 0.000253960 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.951 + y: 630.449 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999959 + w: 0.00906744 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - point: + pose: + position: + x: 342.741 + y: 630.445 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 1200 + - 2500 + - point: + pose: + position: + x: 340.951 + y: 630.409 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999893 + w: 0.0146423 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 339.014 + y: 630.352 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999803 + w: 0.0198557 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 337.016 + y: 630.273 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999732 + w: 0.0231414 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 335.018 + y: 630.180 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999702 + w: 0.0243973 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 333.020 + y: 630.083 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999705 + w: 0.0242858 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 332.731 + y: 630.068 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 16.6667 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + - point: + pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999615 + w: 0.0277448 + longitudinal_velocity_mps: 0.00000 + lateral_velocity_mps: 0.00000 + heading_rate_rps: 0.00000 + is_final: false + lane_ids: + - 2500 + left_bound: + - x: 358.794 + y: 596.731 + z: 100.000 + - x: 358.788 + y: 597.342 + z: 100.000 + - x: 358.758 + y: 600.198 + z: 100.000 + - x: 358.736 + y: 602.392 + z: 100.000 + - x: 358.718 + y: 604.199 + z: 100.000 + - x: 358.698 + y: 606.199 + z: 100.000 + - x: 358.686 + y: 607.440 + z: 100.000 + - x: 358.610 + y: 610.191 + z: 100.000 + - x: 358.512 + y: 612.597 + z: 100.000 + - x: 358.405 + y: 614.096 + z: 100.000 + - x: 358.218 + y: 616.079 + z: 100.000 + - x: 358.086 + y: 617.465 + z: 100.000 + - x: 357.913 + y: 618.622 + z: 100.000 + - x: 357.666 + y: 619.746 + z: 100.000 + - x: 357.285 + y: 620.810 + z: 100.000 + - x: 356.832 + y: 621.812 + z: 100.000 + - x: 356.284 + y: 622.823 + z: 100.000 + - x: 355.663 + y: 623.752 + z: 100.000 + - x: 354.964 + y: 624.613 + z: 100.000 + - x: 354.158 + y: 625.437 + z: 100.000 + - x: 353.306 + y: 626.152 + z: 100.000 + - x: 352.395 + y: 626.788 + z: 100.000 + - x: 351.400 + y: 627.366 + z: 100.000 + - x: 350.389 + y: 627.820 + z: 100.000 + - x: 349.337 + y: 628.212 + z: 100.000 + - x: 348.219 + y: 628.479 + z: 100.000 + - x: 347.083 + y: 628.681 + z: 100.000 + - x: 346.042 + y: 628.729 + z: 100.000 + - x: 344.050 + y: 628.720 + z: 100.000 + - x: 342.764 + y: 628.777 + z: 100.000 + - x: 340.080 + y: 628.723 + z: 100.000 + - x: 338.078 + y: 628.751 + z: 100.000 + - x: 336.079 + y: 628.710 + z: 100.000 + - x: 334.080 + y: 628.670 + z: 100.000 + - x: 332.082 + y: 628.629 + z: 100.000 + - x: 326.872 + y: 628.524 + z: 100.000 + right_bound: + - x: 361.830 + y: 596.762 + z: 100.000 + - x: 361.823 + y: 597.400 + z: 100.000 + - x: 361.792 + y: 600.231 + z: 100.000 + - x: 361.767 + y: 602.446 + z: 100.000 + - x: 361.748 + y: 604.230 + z: 100.000 + - x: 361.726 + y: 606.230 + z: 100.000 + - x: 361.712 + y: 607.493 + z: 100.000 + - x: 361.718 + y: 610.230 + z: 100.000 + - x: 361.754 + y: 612.657 + z: 100.000 + - x: 361.627 + y: 614.331 + z: 100.000 + - x: 361.519 + y: 616.330 + z: 100.000 + - x: 361.452 + y: 617.634 + z: 100.000 + - x: 361.277 + y: 619.056 + z: 100.000 + - x: 360.998 + y: 620.486 + z: 100.000 + - x: 360.581 + y: 621.905 + z: 100.000 + - x: 359.991 + y: 623.293 + z: 100.000 + - x: 359.319 + y: 624.583 + z: 100.000 + - x: 358.490 + y: 625.825 + z: 100.000 + - x: 357.546 + y: 626.984 + z: 100.000 + - x: 356.525 + y: 628.025 + z: 100.000 + - x: 355.377 + y: 628.981 + z: 100.000 + - x: 354.158 + y: 629.845 + z: 100.000 + - x: 352.881 + y: 630.552 + z: 100.000 + - x: 351.515 + y: 631.167 + z: 100.000 + - x: 350.098 + y: 631.615 + z: 100.000 + - x: 348.672 + y: 631.919 + z: 100.000 + - x: 347.236 + y: 632.124 + z: 100.000 + - x: 346.083 + y: 632.098 + z: 100.000 + - x: 344.082 + y: 632.081 + z: 100.000 + - x: 342.719 + y: 632.113 + z: 100.000 + - x: 340.012 + y: 632.058 + z: 100.000 + - x: 338.014 + y: 631.950 + z: 100.000 + - x: 336.015 + y: 631.909 + z: 100.000 + - x: 334.016 + y: 631.869 + z: 100.000 + - x: 332.017 + y: 631.829 + z: 100.000 + - x: 326.807 + y: 631.724 + z: 100.000 +dynamic_object: + header: + stamp: + sec: 73 + nanosec: 980215670 + frame_id: map + objects: + - object_id: + uuid: + - 251 + - 180 + - 23 + - 85 + - 112 + - 118 + - 142 + - 88 + - 241 + - 29 + - 194 + - 27 + - 222 + - 164 + - 130 + - 251 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + covariance: + - 0.299126 + - -8.23923e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -8.23923e-05 + - 0.349251 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00879510 + initial_twist_with_covariance: + twist: + linear: + x: 9.98083 + y: 0.00185652 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00325706 + covariance: + - 0.970951 + - 0.000139981 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000245582 + - 0.000139981 + - 0.00384142 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.000245582 + - 0.00673936 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0118235 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.373 + y: 672.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709614 + w: 0.704590 + - position: + x: 357.520 + y: 677.298 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.696604 + w: 0.717456 + - position: + x: 357.917 + y: 682.299 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.678600 + w: 0.734508 + - position: + x: 358.531 + y: 687.305 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.662668 + w: 0.748913 + - position: + x: 359.266 + y: 692.315 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.653723 + w: 0.756734 + - position: + x: 359.995 + y: 697.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.654217 + w: 0.756307 + - position: + x: 360.590 + y: 702.329 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.664107 + w: 0.747638 + - position: + x: 360.955 + y: 707.330 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.680844 + w: 0.732428 + - position: + x: 361.063 + y: 712.324 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.699424 + w: 0.714707 + - position: + x: 360.983 + y: 717.314 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712725 + w: 0.701443 + - position: + x: 360.864 + y: 722.303 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.746 + y: 727.292 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.627 + y: 732.280 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.508 + y: 737.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.389 + y: 742.258 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.270 + y: 747.247 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.151 + y: 752.236 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 360.032 + y: 757.225 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.913 + y: 762.214 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.794 + y: 767.203 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.675 + y: 772.192 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.557 + y: 777.181 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.438 + y: 782.170 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.319 + y: 787.159 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.200 + y: 792.148 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 359.081 + y: 797.137 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.962 + y: 802.126 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.843 + y: 807.115 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.724 + y: 812.104 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.605 + y: 817.093 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.230769 + - path: + - position: + x: 357.408 + y: 667.312 + z: 100.774 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712257 + w: 0.701919 + - position: + x: 357.340 + y: 672.302 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711946 + w: 0.702234 + - position: + x: 357.296 + y: 677.293 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.710176 + w: 0.704024 + - position: + x: 357.286 + y: 682.284 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707820 + w: 0.706393 + - position: + x: 357.304 + y: 687.276 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705868 + w: 0.708343 + - position: + x: 357.334 + y: 692.269 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704976 + w: 0.709232 + - position: + x: 357.357 + y: 697.261 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.705452 + w: 0.708758 + - position: + x: 357.355 + y: 702.252 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707256 + w: 0.706957 + - position: + x: 357.314 + y: 707.243 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709999 + w: 0.704203 + - position: + x: 357.231 + y: 712.233 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.712949 + w: 0.701216 + - position: + x: 357.119 + y: 717.222 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715047 + w: 0.699076 + - position: + x: 357.000 + y: 722.211 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.881 + y: 727.199 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.762 + y: 732.188 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.643 + y: 737.177 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.524 + y: 742.166 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.405 + y: 747.155 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.286 + y: 752.144 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.167 + y: 757.133 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.049 + y: 762.122 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.930 + y: 767.111 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.811 + y: 772.100 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.692 + y: 777.089 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.573 + y: 782.078 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.454 + y: 787.067 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.335 + y: 792.056 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.216 + y: 797.045 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 355.097 + y: 802.033 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.978 + y: 807.022 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.859 + y: 812.011 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 354.741 + y: 817.000 + z: 100.306 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.769231 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.89999 + y: 0.500442 + z: 0.612431 + - object_id: + uuid: + - 218 + - 141 + - 152 + - 171 + - 185 + - 127 + - 84 + - 175 + - 69 + - 40 + - 199 + - 89 + - 96 + - 107 + - 61 + - 243 + existence_probability: 0.00000 + classification: + - label: 6 + probability: 1.00000 + kinematics: + initial_pose_with_covariance: + pose: + position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + covariance: + - 0.0636258 + - -0.000170219 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000170219 + - 0.0826995 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00394708 + initial_twist_with_covariance: + twist: + linear: + x: 10.0303 + y: -0.00247386 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: -0.00434011 + covariance: + - 0.579437 + - -9.81341e-05 + - 0.00000 + - 0.00000 + - 0.00000 + - -0.000172165 + - -9.81341e-05 + - 0.00311875 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.0100000 + - 0.00000 + - -0.000172165 + - 0.00547149 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00959910 + initial_acceleration_with_covariance: + accel: + linear: + x: 0.00000 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + predicted_paths: + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.224 + y: 608.637 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.706633 + w: 0.707581 + - position: + x: 358.279 + y: 613.653 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.703235 + w: 0.710958 + - position: + x: 358.397 + y: 618.671 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698737 + w: 0.715379 + - position: + x: 358.567 + y: 623.690 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.695068 + w: 0.718944 + - position: + x: 358.758 + y: 628.710 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.693532 + w: 0.720426 + - position: + x: 358.932 + y: 633.729 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.694735 + w: 0.719266 + - position: + x: 359.054 + y: 638.747 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.698453 + w: 0.715656 + - position: + x: 359.097 + y: 643.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.704045 + w: 0.710155 + - position: + x: 359.057 + y: 648.778 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.709938 + w: 0.704265 + - position: + x: 358.958 + y: 653.791 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714096 + w: 0.700048 + - position: + x: 358.849 + y: 658.805 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714730 + w: 0.699401 + - position: + x: 358.740 + y: 663.817 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.714768 + w: 0.699361 + - position: + x: 358.620 + y: 668.830 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.501 + y: 673.843 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.381 + y: 678.856 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.262 + y: 683.870 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.142 + y: 688.883 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 358.023 + y: 693.896 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.903 + y: 698.909 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.784 + y: 703.923 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.664 + y: 708.936 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.545 + y: 713.950 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.425 + y: 718.963 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.306 + y: 723.977 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.186 + y: 728.991 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 357.067 + y: 734.004 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.947 + y: 739.018 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.828 + y: 744.032 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.708 + y: 749.045 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + - position: + x: 356.589 + y: 754.059 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.715481 + w: 0.698632 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + - path: + - position: + x: 358.218 + y: 603.622 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707443 + w: 0.706770 + - position: + x: 358.206 + y: 608.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707930 + w: 0.706283 + - position: + x: 358.149 + y: 613.642 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.711130 + w: 0.703060 + - position: + x: 358.010 + y: 618.635 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.716853 + w: 0.697224 + - position: + x: 357.024 + y: 623.117 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.779378 + w: 0.626554 + - position: + x: 354.441 + y: 627.006 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.881248 + w: 0.472653 + - position: + x: 350.340 + y: 629.665 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.958935 + w: 0.283625 + - position: + x: 345.469 + y: 630.694 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.994592 + w: 0.103864 + - position: + x: 340.508 + y: 630.814 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999926 + w: 0.0121264 + - position: + x: 335.554 + y: 630.768 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999989 + w: 0.00460609 + - position: + x: 330.597 + y: 630.677 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999957 + w: 0.00925679 + - position: + x: 325.638 + y: 630.576 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 320.674 + y: 630.476 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 315.708 + y: 630.376 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 310.737 + y: 630.276 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 305.763 + y: 630.175 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 300.785 + y: 630.075 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 295.803 + y: 629.974 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 290.817 + y: 629.874 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 285.828 + y: 629.773 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 280.835 + y: 629.672 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 275.839 + y: 629.571 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 270.840 + y: 629.470 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 265.838 + y: 629.370 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 260.833 + y: 629.268 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 255.825 + y: 629.167 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 250.816 + y: 629.066 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 245.805 + y: 628.965 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 240.792 + y: 628.864 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 235.778 + y: 628.763 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + - position: + x: 230.764 + y: 628.662 + z: 100.496 + orientation: + x: 0.00000 + y: 0.00000 + z: -0.999949 + w: 0.0100898 + time_step: + sec: 0 + nanosec: 500000000 + confidence: 0.500000 + shape: + type: 0 + footprint: + points: [] + dimensions: + x: 1.90000 + y: 0.644288 + z: 0.991445 +traffic_signal: + stamp: + sec: 0 + nanosec: 0 + traffic_light_groups: [] +route: + header: + stamp: + sec: 19 + nanosec: 549630704 + frame_id: map + start_pose: + position: + x: 360.481 + y: 591.844 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.721675 + w: 0.692232 + goal_pose: + position: + x: 331.733 + y: 630.013 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.999615 + w: -0.0277448 + segments: + - preferred_primitive: + id: 1010 + primitive_type: "" + primitives: + - id: 1010 + primitive_type: lane + - preferred_primitive: + id: 1200 + primitive_type: "" + primitives: + - id: 1200 + primitive_type: lane + - preferred_primitive: + id: 2500 + primitive_type: "" + primitives: + - id: 2500 + primitive_type: lane + - id: 2501 + primitive_type: lane + uuid: + uuid: + - 83 + - 230 + - 40 + - 201 + - 182 + - 232 + - 31 + - 237 + - 204 + - 91 + - 39 + - 90 + - 153 + - 179 + - 6 + - 123 + allow_modification: false +operation_mode: + stamp: + sec: 65 + nanosec: 819714871 + mode: 2 + is_autoware_control_enabled: true + is_in_transition: false + is_stop_mode_available: true + is_autonomous_mode_available: true + is_local_mode_available: true + is_remote_mode_available: true +self_acceleration: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: /base_link + accel: + accel: + linear: + x: -1.01094 + y: 0.00400509 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00000 + covariance: + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00100000 +self_odometry: + header: + stamp: + sec: 74 + nanosec: 42641063 + frame_id: map + child_frame_id: base_link + pose: + pose: + position: + x: 360.302 + y: 604.493 + z: 100.000 + orientation: + x: 0.00000 + y: 0.00000 + z: 0.707950 + w: 0.706263 + covariance: + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.000100000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + twist: + twist: + linear: + x: 2.31221 + y: 0.00000 + z: 0.00000 + angular: + x: 0.00000 + y: 0.00000 + z: 0.00173214 + covariance: + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 + - 0.00000 From 7faa5cf51330742515ee4c4ae0ef055494f24419 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Wed, 8 Jan 2025 13:54:00 +0900 Subject: [PATCH 2/2] fix Signed-off-by: Mamoru Sobue --- .../test/test_util.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp index 2586019797ddd..5c2a239e4142f 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_blind_spot_module/test/test_util.cpp @@ -439,7 +439,9 @@ TEST_F(TestWithShoulderData, generateBlindSpotLaneletsShoulder_left) int main(int argc, char ** argv) { +#ifdef EXPORT_TEST_PLOT_FIGURE py::scoped_interpreter guard{}; +#endif rclcpp::init(0, nullptr); testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS();