From 442bf7d38c64df31e9db397a3d6b83d4df3dc4b1 Mon Sep 17 00:00:00 2001 From: beginningfan Date: Sat, 25 Nov 2023 14:04:48 +0800 Subject: [PATCH 1/3] feat(autoware.universe): replace autoware_auto_perception_msg with autoware_erception_msg Signed-off-by: beginningfan --- .../README.md | 2 +- .../object_polygon_detail.hpp | 45 ++ .../object_polygon_display_base.hpp | 45 ++ .../predicted_objects_display.hpp | 6 +- .../package.xml | 1 + .../object_polygon_detail.cpp | 425 ++++++++++++++++++ .../predicted_objects_display.cpp | 2 +- common/autoware_auto_tf2/CMakeLists.txt | 28 +- .../component_interface_specs/CMakeLists.txt | 2 +- .../component_interface_specs/perception.hpp | 4 +- common/component_interface_specs/package.xml | 2 +- .../predicted_path_utils.hpp | 12 +- common/object_recognition_utils/package.xml | 2 +- .../src/predicted_path_utils.cpp | 12 +- .../test/src/test_predicted_path_utils.cpp | 2 +- .../geometry/boost_polygon_utils.hpp | 6 +- common/tier4_autoware_utils/package.xml | 1 + .../src/geometry/boost_polygon_utils.cpp | 64 ++- .../src/geometry/test_boost_polygon_utils.cpp | 12 +- .../collision_checker.hpp | 4 +- .../predicted_path_checker_node.hpp | 4 +- .../include/predicted_path_checker/utils.hpp | 12 +- evaluator/planning_evaluator/README.md | 2 +- .../metrics/obstacle_metrics.hpp | 4 +- .../planning_evaluator/metrics_calculator.hpp | 4 +- .../planning_evaluator_node.hpp | 4 +- evaluator/planning_evaluator/package.xml | 2 +- .../test/test_planning_evaluator_node.cpp | 8 +- perception/map_based_prediction/README.md | 2 +- .../map_based_prediction_node.hpp | 12 +- .../map_based_prediction/path_generator.hpp | 12 +- perception/map_based_prediction/package.xml | 2 +- .../src/map_based_prediction_node.cpp | 11 +- perception/radar_object_tracker/README.md | 2 +- planning/behavior_path_planner/README.md | 2 +- .../behavior_path_planner_node.hpp | 6 +- .../behavior_path_planner/data_manager.hpp | 6 +- .../marker_utils/avoidance/debug.hpp | 4 +- .../marker_utils/utils.hpp | 8 +- .../avoidance/avoidance_module.hpp | 2 +- .../dynamic_avoidance_module.hpp | 12 +- .../utils/avoidance/avoidance_module_data.hpp | 6 +- .../drivable_area_expansion/footprints.hpp | 4 +- .../utils/drivable_area_expansion/types.hpp | 4 +- .../geometric_parallel_parking.hpp | 2 +- .../utils/goal_planner/util.hpp | 8 +- .../utils/lane_change/utils.hpp | 8 +- .../path_safety_checker/objects_filtering.hpp | 8 +- .../path_safety_checker_parameters.hpp | 6 +- .../path_safety_checker/safety_check.hpp | 10 +- .../common_module_data.hpp | 4 +- .../utils/start_planner/util.hpp | 8 +- .../behavior_path_planner/utils/utils.hpp | 18 +- planning/behavior_path_planner/package.xml | 1 - .../src/behavior_path_planner_node.cpp | 6 +- .../src/marker_utils/avoidance/debug.cpp | 2 +- .../src/marker_utils/lane_change/debug.cpp | 2 +- .../src/scene_module/avoidance/manager.cpp | 4 +- .../dynamic_avoidance_module.cpp | 12 +- .../src/scene_module/lane_change/manager.cpp | 2 +- .../drivable_area_expansion/footprints.cpp | 2 +- .../src/utils/lane_change/utils.cpp | 4 +- .../path_safety_checker/objects_filtering.cpp | 2 +- .../behavior_path_planner/src/utils/utils.cpp | 4 +- .../test/test_safety_check.cpp | 4 +- .../package.xml | 2 +- .../src/scene.cpp | 16 +- .../src/scene.hpp | 14 +- .../package.xml | 1 - .../src/scene_crosswalk.hpp | 8 +- .../src/util.cpp | 2 +- .../package.xml | 1 - .../src/scene_intersection.cpp | 20 +- .../src/scene_intersection.hpp | 2 +- .../src/scene_merge_from_private_road.hpp | 4 +- .../src/util.cpp | 18 +- .../src/util.hpp | 6 +- .../src/util_type.hpp | 16 +- .../package.xml | 2 +- .../src/scene_no_stopping_area.cpp | 14 +- .../src/scene_no_stopping_area.hpp | 10 +- .../package.xml | 2 +- .../src/grid_utils.hpp | 10 +- .../src/manager.hpp | 4 +- .../src/occlusion_spot_utils.hpp | 12 +- .../src/scene_occlusion_spot.cpp | 2 +- .../src/scene_occlusion_spot.hpp | 4 +- .../test/src/test_occlusion_spot_utils.cpp | 4 +- .../package.xml | 2 +- .../src/decisions.cpp | 4 +- .../src/decisions.hpp | 6 +- .../src/filter_predicted_objects.hpp | 8 +- .../src/manager.hpp | 4 +- .../src/scene_out_of_lane.hpp | 2 +- .../src/types.hpp | 4 +- planning/behavior_velocity_planner/README.md | 2 +- .../behavior_velocity_planner/package.xml | 2 +- .../behavior_velocity_planner/src/node.cpp | 4 +- .../behavior_velocity_planner/src/node.hpp | 6 +- .../src/planner_manager.hpp | 2 +- .../planner_data.hpp | 4 +- .../utilization/debug.hpp | 4 +- .../utilization/util.hpp | 4 +- .../src/utilization/debug.cpp | 2 +- .../README.md | 4 +- .../package.xml | 2 +- .../src/dynamic_obstacle.hpp | 6 +- .../src/scene.hpp | 2 +- .../src/utils.cpp | 2 +- .../src/utils.hpp | 8 +- planning/costmap_generator/README.md | 2 +- .../costmap_generator/costmap_generator.hpp | 10 +- .../costmap_generator/objects_to_costmap.hpp | 10 +- .../costmap_generator_node.cpp | 14 +- .../costmap_generator/objects_to_costmap.cpp | 14 +- planning/costmap_generator/package.xml | 2 +- .../test/test_objects_to_costmap.cpp | 10 +- planning/obstacle_cruise_planner/README.md | 2 +- .../obstacle_cruise_planner/type_alias.hpp | 14 +- planning/obstacle_cruise_planner/package.xml | 2 +- planning/obstacle_stop_planner/README.md | 2 +- .../adaptive_cruise_control.hpp | 8 +- .../include/obstacle_stop_planner/node.hpp | 6 +- .../obstacle_stop_planner/planner_data.hpp | 4 +- .../obstacle_stop_planner/planner_utils.hpp | 12 +- planning/obstacle_stop_planner/package.xml | 2 +- .../src/adaptive_cruise_control.cpp | 6 +- planning/obstacle_stop_planner/src/node.cpp | 38 +- .../src/planner_utils.cpp | 16 +- planning/obstacle_velocity_limiter/README.md | 2 +- .../obstacle_velocity_limiter.hpp | 4 +- .../obstacle_velocity_limiter_node.hpp | 2 +- .../obstacle_velocity_limiter/obstacles.hpp | 4 +- .../obstacle_velocity_limiter/types.hpp | 4 +- .../obstacle_velocity_limiter/package.xml | 2 +- .../src/obstacle_velocity_limiter.cpp | 2 +- .../src/obstacles.cpp | 2 +- .../test/test_collision_distance.cpp | 8 +- .../perception_replayer_common.py | 10 +- .../planning_interface_test_manager.hpp | 4 +- .../path_sampler/README.md | 2 +- .../include/path_sampler/prepare_inputs.hpp | 2 +- .../include/path_sampler/type_alias.hpp | 4 +- .../path_sampler/package.xml | 1 + .../static_centerline_optimizer_node.hpp | 4 +- .../type_alias.hpp | 4 +- .../static_centerline_optimizer/package.xml | 2 +- planning/surround_obstacle_checker/README.md | 2 +- .../surround_obstacle_checker/node.hpp | 6 +- .../surround_obstacle_checker/package.xml | 2 +- .../surround_obstacle_checker/src/node.cpp | 2 +- .../surround_obstacle_checker-design.ja.md | 2 +- .../dummy_perception_publisher/package.xml | 2 +- .../src/empty_objects_publisher.cpp | 8 +- 154 files changed, 1017 insertions(+), 431 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index cb5deb48f411c..dcc4c84a56c0a 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -47,7 +47,7 @@ Overwrite tracking results with detection results. | Name | Type | Description | | ---- | ------------------------------------------------------ | ----------------------- | -| | `autoware_auto_perception_msgs::msg::PredictedObjects` | prediction result array | +| | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | #### Visualization Result diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 5d99b8a463711..2ddd2dd07b898 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -19,6 +19,9 @@ #include #include +#include +#include +#include #include #include #include @@ -84,12 +87,24 @@ get_shape_marker_ptr( const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_2d_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width); + /// \brief Convert the given polygon into a marker representing the shape in 3d /// \param centroid Centroid position of the shape in Object.header.frame_id frame /// \return Marker ptr. Id and header will have to be set by the caller @@ -127,27 +142,48 @@ get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_predicted_path_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple = false); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, const std_msgs::msg::ColorRGBA & path_confidence_color); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC visualization_msgs::msg::Marker::SharedPtr +get_path_confidence_marker_ptr( + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_bounding_box_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_bounding_box_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_cylinder_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_cylinder_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, @@ -156,14 +192,23 @@ AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_circle_line_list( AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_polygon_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_2d_polygon_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points); AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, std::vector & points, const bool is_simple = false); +AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC void calc_path_line_list( + const autoware_perception_msgs::msg::PredictedPath & paths, + std::vector & points, const bool is_simple = false); /// \brief Convert Point32 to Point /// \param val Point32 to be converted diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp index 10bf11847c2f5..22ced533f261c 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_display_base.hpp @@ -177,6 +177,23 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + template + std::optional get_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const ClassificationContainerT & labels, const double & line_width) const + { + const std_msgs::msg::ColorRGBA color_rgba = get_color_rgba(labels); + if (m_display_type_property->getOptionInt() == 0) { + return detail::get_shape_marker_ptr(shape_msg, centroid, orientation, color_rgba, line_width); + } else if (m_display_type_property->getOptionInt() == 1) { + return detail::get_2d_shape_marker_ptr( + shape_msg, centroid, orientation, color_rgba, line_width); + } else { + return std::nullopt; + } + } + template visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, @@ -278,6 +295,21 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase return std::nullopt; } } + std::optional get_predicted_path_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_predicted_paths_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA predicted_path_color = get_color_from_uuid(uuid_str); + return detail::get_predicted_path_marker_ptr( + shape, predicted_path, predicted_path_color, + m_simple_visualize_mode_property->getOptionInt() == 1); + } else { + return std::nullopt; + } + } std::optional get_path_confidence_marker_ptr( const unique_identifier_msgs::msg::UUID & uuid, @@ -292,6 +324,19 @@ class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC ObjectPolygonDisplayBase } } + std::optional get_path_confidence_marker_ptr( + const unique_identifier_msgs::msg::UUID & uuid, + const autoware_perception_msgs::msg::PredictedPath & predicted_path) const + { + if (m_display_path_confidence_property.getBool()) { + const std::string uuid_str = uuid_to_string(uuid); + const std_msgs::msg::ColorRGBA path_confidence_color = get_color_from_uuid(uuid_str); + return detail::get_path_confidence_marker_ptr(predicted_path, path_confidence_color); + } else { + return std::nullopt; + } + } + /// \brief Get color and alpha values based on the given list of classification values /// \tparam ClassificationContainerT Container of ObjectClassification /// \param labels list of classifications diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp index 2896286970217..8fcc2020566c3 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/predicted_objects_display.hpp @@ -16,7 +16,7 @@ #include -#include +#include #include #include @@ -39,12 +39,12 @@ namespace object_detection { /// \brief Class defining rviz plugin to visualize PredictedObjects class AUTOWARE_AUTO_PERCEPTION_RVIZ_PLUGIN_PUBLIC PredictedObjectsDisplay -: public ObjectPolygonDisplayBase +: public ObjectPolygonDisplayBase { Q_OBJECT public: - using PredictedObjects = autoware_auto_perception_msgs::msg::PredictedObjects; + using PredictedObjects = autoware_perception_msgs::msg::PredictedObjects; PredictedObjectsDisplay(); ~PredictedObjectsDisplay() diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index c1ad3e3df140e..e5f8bec2c8e49 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -17,6 +17,7 @@ libboost-dev qtbase5-dev + autoware_perception_msgs autoware_auto_perception_msgs rviz_common rviz_default_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index 894e377a6f851..d34b3dfd80423 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -68,6 +68,26 @@ visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_path_confidence_marker_ptr( + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & path_confidence_color) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker_ptr->ns = std::string("path confidence"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = 0.5; + marker_ptr->scale.y = 0.5; + marker_ptr->scale.z = 0.5; + marker_ptr->pose = initPose(); + marker_ptr->color = path_confidence_color; + marker_ptr->pose.position = predicted_path.path.back().position; + marker_ptr->text = std::to_string(predicted_path.confidence); + marker_ptr->color.a = 0.5; + return marker_ptr; +} + visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape, const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, @@ -88,6 +108,26 @@ visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( } return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_predicted_path_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape, + const autoware_perception_msgs::msg::PredictedPath & predicted_path, + const std_msgs::msg::ColorRGBA & predicted_path_color, const bool is_simple) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + marker_ptr->ns = std::string("path"); + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->pose = initPose(); + marker_ptr->color = predicted_path_color; + marker_ptr->color.a = 0.6; + marker_ptr->scale.x = 0.015; + calc_path_line_list(predicted_path, marker_ptr->points, is_simple); + for (size_t k = 0; k < marker_ptr->points.size(); ++k) { + marker_ptr->points.at(k).z -= shape.dimensions.z / 2.0; + } + return marker_ptr; +} visualization_msgs::msg::Marker::SharedPtr get_twist_marker_ptr( const geometry_msgs::msg::PoseWithCovariance & pose_with_covariance, @@ -274,6 +314,39 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->ns = std::string("shape"); + + using autoware_perception_msgs::msg::Shape; + if (shape_msg.type == Shape::BOUNDING_BOX) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_bounding_box_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::CYLINDER) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_cylinder_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::POLYGON) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_polygon_line_list(shape_msg, marker_ptr->points); + } else { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_polygon_line_list(shape_msg, marker_ptr->points); + } + + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = line_width; + marker_ptr->color = color_rgba; + + return marker_ptr; +} + + visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -306,6 +379,39 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( return marker_ptr; } +visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( + const autoware_perception_msgs::msg::Shape & shape_msg, + const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, + const std_msgs::msg::ColorRGBA & color_rgba, const double & line_width) +{ + auto marker_ptr = std::make_shared(); + marker_ptr->ns = std::string("shape"); + + using autoware_perception_msgs::msg::Shape; + if (shape_msg.type == Shape::BOUNDING_BOX) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_bounding_box_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::CYLINDER) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_cylinder_bottom_line_list(shape_msg, marker_ptr->points); + } else if (shape_msg.type == Shape::POLYGON) { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } else { + marker_ptr->type = visualization_msgs::msg::Marker::LINE_LIST; + calc_2d_polygon_bottom_line_list(shape_msg, marker_ptr->points); + } + + marker_ptr->action = visualization_msgs::msg::Marker::MODIFY; + marker_ptr->pose = to_pose(centroid, orientation); + marker_ptr->lifetime = rclcpp::Duration::from_seconds(0.2); + marker_ptr->scale.x = line_width; + marker_ptr->color = color_rgba; + + return marker_ptr; +} + + void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -422,6 +528,122 @@ void calc_bounding_box_line_list( points.push_back(point); } +void calc_bounding_box_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + geometry_msgs::msg::Point point; + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + // up surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + + // down surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); +} + void calc_2d_bounding_box_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -465,6 +687,50 @@ void calc_2d_bounding_box_bottom_line_list( points.push_back(point); } +void calc_2d_bounding_box_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + geometry_msgs::msg::Point point; + // down surface + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = -shape.dimensions.x / 2.0; + point.y = shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + + point.x = shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = -shape.dimensions.x / 2.0; + point.y = -shape.dimensions.y / 2.0; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); +} + + void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -508,6 +774,49 @@ void calc_cylinder_line_list( } } +void calc_cylinder_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double radius = shape.dimensions.x * 0.5; + { + constexpr int n = 20; + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + center.z = -shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + } + { + constexpr int n = 4; + for (int i = 0; i < n; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.y = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.z = shape.dimensions.z * 0.5; + points.push_back(point); + point.x = std::cos( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.y = std::sin( + (static_cast(i) / static_cast(n)) * 2.0 * M_PI + + M_PI / static_cast(n)) * + radius; + point.z = -shape.dimensions.z * 0.5; + points.push_back(point); + } + } +} + void calc_2d_cylinder_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -523,6 +832,21 @@ void calc_2d_cylinder_bottom_line_list( } } +void calc_2d_cylinder_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + const double radius = shape.dimensions.x * 0.5; + { + constexpr int n = 20; + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = -shape.dimensions.z * 0.5; + calc_circle_line_list(center, radius, points, n); + } +} + void calc_circle_line_list( const geometry_msgs::msg::Point center, const double radius, std::vector & points, const int n) @@ -609,6 +933,59 @@ void calc_polygon_line_list( } } +void calc_polygon_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + if (shape.footprint.points.size() < 2) { + RCLCPP_WARN( + rclcpp::get_logger("ObjectPolygonDisplayBase"), + "there are no enough footprint to visualize polygon"); + return; + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } +} + void calc_2d_polygon_bottom_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -635,6 +1012,32 @@ void calc_2d_polygon_bottom_line_list( points.push_back(point); } } +void calc_2d_polygon_bottom_line_list( + const autoware_perception_msgs::msg::Shape & shape, + std::vector & points) +{ + if (shape.footprint.points.size() < 2) { + RCLCPP_WARN( + rclcpp::get_logger("ObjectPolygonDisplayBase"), + "there are no enough footprint to visualize polygon"); + return; + } + for (size_t i = 0; i < shape.footprint.points.size(); ++i) { + geometry_msgs::msg::Point point; + point.x = shape.footprint.points.at(i).x; + point.y = shape.footprint.points.at(i).y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + point.x = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .x; + point.y = shape.footprint.points + .at(static_cast(i + 1) % static_cast(shape.footprint.points.size())) + .y; + point.z = -shape.dimensions.z / 2.0; + points.push_back(point); + } +} void calc_path_line_list( const autoware_auto_perception_msgs::msg::PredictedPath & paths, @@ -658,6 +1061,28 @@ void calc_path_line_list( } } +void calc_path_line_list( + const autoware_perception_msgs::msg::PredictedPath & paths, + std::vector & points, const bool is_simple) +{ + const int circle_line_num = is_simple ? 5 : 10; + + for (int i = 0; i < static_cast(paths.path.size()) - 1; ++i) { + geometry_msgs::msg::Point point; + point.x = paths.path.at(i).position.x; + point.y = paths.path.at(i).position.y; + point.z = paths.path.at(i).position.z; + points.push_back(point); + point.x = paths.path.at(i + 1).position.x; + point.y = paths.path.at(i + 1).position.y; + point.z = paths.path.at(i + 1).position.z; + points.push_back(point); + if (!is_simple || i % 2 == 0) { + calc_circle_line_list(point, 0.25, points, circle_line_num); + } + } +} + } // namespace detail } // namespace object_detection } // namespace rviz_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp index 2cc5397d18721..35da353599d00 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/predicted_objects_display.cpp @@ -232,7 +232,7 @@ void PredictedObjectsDisplay::update(float wall_dt, float ros_dt) lock.unlock(); - ObjectPolygonDisplayBase::update( + ObjectPolygonDisplayBase::update( wall_dt, ros_dt); } diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index a8ae9ec2d962e..81af3ee184c6a 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -4,19 +4,19 @@ project(autoware_auto_tf2) find_package(autoware_cmake REQUIRED) autoware_package() -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) - target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") - ament_target_dependencies(test_tf2_autoware_auto_msgs - "autoware_auto_common" - "autoware_auto_perception_msgs" - "autoware_auto_system_msgs" - "autoware_auto_geometry_msgs" - "geometry_msgs" - "tf2" - "tf2_geometry_msgs" - "tf2_ros" - ) -endif() +# if(BUILD_TESTING) +# ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) +# target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") +# ament_target_dependencies(test_tf2_autoware_auto_msgs +# "autoware_auto_common" +# "autoware_auto_perception_msgs" +# "autoware_auto_system_msgs" +# "autoware_auto_geometry_msgs" +# "geometry_msgs" +# "tf2" +# "tf2_geometry_msgs" +# "tf2_ros" +# ) +# endif() ament_auto_package() diff --git a/common/component_interface_specs/CMakeLists.txt b/common/component_interface_specs/CMakeLists.txt index b519a0ddce86a..8525460913738 100644 --- a/common/component_interface_specs/CMakeLists.txt +++ b/common/component_interface_specs/CMakeLists.txt @@ -18,7 +18,7 @@ include_directories( ${nav_msgs_INCLUDE_DIRS} ${tier4_system_msgs_INCLUDE_DIRS} ${tier4_vehicle_msgs_INCLUDE_DIRS} - ${autoware_auto_perception_msgs_INCLUDE_DIRS} + ${autoware_perception_msgs_INCLUDE_DIRS} ${tier4_map_msgs_INCLUDE_DIRS} ) diff --git a/common/component_interface_specs/include/component_interface_specs/perception.hpp b/common/component_interface_specs/include/component_interface_specs/perception.hpp index 8665b9c35157d..0c72dbdb12078 100644 --- a/common/component_interface_specs/include/component_interface_specs/perception.hpp +++ b/common/component_interface_specs/include/component_interface_specs/perception.hpp @@ -17,14 +17,14 @@ #include -#include +#include namespace perception_interface { struct ObjectRecognition { - using Message = autoware_auto_perception_msgs::msg::PredictedObjects; + using Message = autoware_perception_msgs::msg::PredictedObjects; static constexpr char name[] = "/perception/object_recognition/objects"; static constexpr size_t depth = 1; static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index 93f77c651869d..c8063bfb74d21 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -20,7 +20,7 @@ ament_cmake_test autoware_adapi_v1_msgs - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs autoware_planning_msgs diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 73cdce6c6444e..4412e10485c77 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ namespace object_recognition_utils * @return interpolated pose */ boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time); + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time); /** * @brief Resampling predicted path by time step vector. Note this function does not substitute @@ -44,8 +44,8 @@ boost::optional calcInterpolatedPose( * time_step*(num_of_path_points)] * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); @@ -57,8 +57,8 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( * @param sampling_horizon sampling time horizon * @return resampled path */ -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy = true, const bool use_spline_for_z = false); } // namespace object_recognition_utils diff --git a/common/object_recognition_utils/package.xml b/common/object_recognition_utils/package.xml index 2f2472515ebad..20eafe4c0f898 100644 --- a/common/object_recognition_utils/package.xml +++ b/common/object_recognition_utils/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs interpolation libboost-dev diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 81a22c98d88e5..1f08afb86eab9 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -23,7 +23,7 @@ namespace object_recognition_utils { boost::optional calcInterpolatedPose( - const autoware_auto_perception_msgs::msg::PredictedPath & path, const double relative_time) + const autoware_perception_msgs::msg::PredictedPath & path, const double relative_time) { // Check if relative time is in the valid range if (path.path.empty() || relative_time < 0.0) { @@ -45,8 +45,8 @@ boost::optional calcInterpolatedPose( return boost::none; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const std::vector & resampled_time, const bool use_spline_for_xy, const bool use_spline_for_z) { @@ -83,7 +83,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( const auto interpolated_z = use_spline_for_z ? spline(z) : lerp(z); const auto interpolated_quat = slerp(quat); - autoware_auto_perception_msgs::msg::PredictedPath resampled_path; + autoware_perception_msgs::msg::PredictedPath resampled_path; const auto resampled_size = std::min(resampled_path.path.max_size(), resampled_time.size()); resampled_path.confidence = path.confidence; resampled_path.path.resize(resampled_size); @@ -99,8 +99,8 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( return resampled_path; } -autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & path, +autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { diff --git a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp index b466273f4defd..a7e6737cd2d42 100644 --- a/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp +++ b/common/object_recognition_utils/test/src/test_predicted_path_utils.cpp @@ -25,7 +25,7 @@ constexpr double epsilon = 1e-06; namespace { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::transformPoint; diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 5d9001affa308..0f823a3a0a605 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -18,7 +18,7 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include -#include +#include #include #include @@ -35,11 +35,13 @@ geometry_msgs::msg::Polygon rotatePolygon( /// @param[in] angle angle of rotation [rad] /// @return rotated polygon Polygon2d rotatePolygon(const Polygon2d & polygon, const double angle); +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape); Polygon2d toPolygon2d( const geometry_msgs::msg::Pose & pose, const autoware_auto_perception_msgs::msg::Shape & shape); Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & object); Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); -Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); +Polygon2d toPolygon2d(const autoware_perception_msgs::msg::PredictedObject & object); Polygon2d toFootprint( const geometry_msgs::msg::Pose & base_link_pose, const double base_to_front, const double base_to_rear, const double width); diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index e37cd0cc33975..57e52fb4d4a82 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -13,6 +13,7 @@ autoware_cmake autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs builtin_interfaces diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index 79c9f9937cd4d..dc06037fcb5cb 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -179,6 +179,68 @@ Polygon2d toPolygon2d( return isClockwise(polygon) ? polygon : inverseClockwise(polygon); } +Polygon2d toPolygon2d( + const geometry_msgs::msg::Pose & pose, const autoware_perception_msgs::msg::Shape & shape) +{ + Polygon2d polygon; + + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { + const auto point0 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point1 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, shape.dimensions.y / 2.0, 0.0) + .position; + const auto point2 = tier4_autoware_utils::calcOffsetPose( + pose, -shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + const auto point3 = tier4_autoware_utils::calcOffsetPose( + pose, shape.dimensions.x / 2.0, -shape.dimensions.y / 2.0, 0.0) + .position; + + appendPointToPolygon(polygon, point0); + appendPointToPolygon(polygon, point1); + appendPointToPolygon(polygon, point2); + appendPointToPolygon(polygon, point3); + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { + const double radius = shape.dimensions.x / 2.0; + constexpr int circle_discrete_num = 6; + for (int i = 0; i < circle_discrete_num; ++i) { + geometry_msgs::msg::Point point; + point.x = std::cos( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.x; + point.y = std::sin( + (static_cast(i) / static_cast(circle_discrete_num)) * 2.0 * M_PI + + M_PI / static_cast(circle_discrete_num)) * + radius + + pose.position.y; + appendPointToPolygon(polygon, point); + } + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { + const double poly_yaw = tf2::getYaw(pose.orientation); + const auto rotated_footprint = rotatePolygon(shape.footprint, poly_yaw); + for (const auto rel_point : rotated_footprint.points) { + geometry_msgs::msg::Point abs_point; + abs_point.x = pose.position.x + rel_point.x; + abs_point.y = pose.position.y + rel_point.y; + + appendPointToPolygon(polygon, abs_point); + } + } else { + throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); + } + + // NOTE: push back the first point in order to close polygon + if (!polygon.outer().empty()) { + appendPointToPolygon(polygon, polygon.outer().front()); + } + + return isClockwise(polygon) ? polygon : inverseClockwise(polygon); +} + tier4_autoware_utils::Polygon2d toPolygon2d( const autoware_auto_perception_msgs::msg::DetectedObject & object) { @@ -194,7 +256,7 @@ tier4_autoware_utils::Polygon2d toPolygon2d( } tier4_autoware_utils::Polygon2d toPolygon2d( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { return tier4_autoware_utils::toPolygon2d( object.kinematics.initial_pose_with_covariance.pose, object.shape); diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 7370c3b650887..7a2e5073ee498 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -137,8 +137,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; shape.dimensions.x = x; shape.dimensions.y = y; @@ -159,8 +159,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double diameter = 1.0; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::CYLINDER; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::CYLINDER; shape.dimensions.x = diameter; const auto poly = toPolygon2d(pose, shape); @@ -183,8 +183,8 @@ TEST(boost_geometry, boost_toPolygon2d) const double y = 0.5; const auto pose = createPose(1.0, 1.0, M_PI_4); - autoware_auto_perception_msgs::msg::Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + autoware_perception_msgs::msg::Shape shape; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.push_back(createPoint32(-x, -y)); shape.footprint.points.push_back(createPoint32(-x, y)); shape.footprint.points.push_back(createPoint32(x, y)); diff --git a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp index d21b11e5b68de..f28578237e4c1 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp @@ -45,8 +45,8 @@ namespace autoware::motion::control::predicted_path_checker using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index e3c3b5cccfb8f..ed19871aae1cf 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -27,7 +27,7 @@ #include #include -#include +#include #include #include #include @@ -50,7 +50,7 @@ namespace autoware::motion::control::predicted_path_checker using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; using tier4_autoware_utils::Point2d; diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 75e90e624a17e..979311c219fa3 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -39,8 +39,8 @@ namespace utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -85,14 +85,14 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( const double & base_to_width); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertObjToPolygon(const PredictedObject & obj); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); void getCurrentObjectPose( PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, diff --git a/evaluator/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md index 23c8101429c35..82c1886d1c68b 100644 --- a/evaluator/planning_evaluator/README.md +++ b/evaluator/planning_evaluator/README.md @@ -52,7 +52,7 @@ Adding a new metric `M` requires the following steps: | ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | | `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | | `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Obstacles | +| `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Obstacles | ### Outputs diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp index 848d92c91f13e..23e5341f20c09 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp @@ -17,14 +17,14 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; /** diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 851678e55d755..6188ab98f5543 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -18,7 +18,7 @@ #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -28,7 +28,7 @@ namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::PoseWithUuidStamped; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index 888eea855295c..cce95519babf2 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -21,7 +21,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" @@ -36,7 +36,7 @@ namespace planning_diagnostics { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::PoseWithUuidStamped; diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index fb3270fb0d89b..003373f5c419f 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -13,7 +13,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_planning_msgs diagnostic_msgs diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index 4f2ab014a79d6..eb63e641cb323 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -20,7 +20,7 @@ #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -35,7 +35,7 @@ using EvalNode = planning_diagnostics::PlanningEvaluatorNode; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; -using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using Objects = autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using nav_msgs::msg::Odometry; @@ -409,7 +409,7 @@ TEST_F(EvalTest, TestObstacleDistance) { setTargetMetric(planning_diagnostics::Metric::obstacle_distance); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); @@ -425,7 +425,7 @@ TEST_F(EvalTest, TestObstacleTTC) { setTargetMetric(planning_diagnostics::Metric::obstacle_ttc); Objects objs; - autoware_auto_perception_msgs::msg::PredictedObject obj; + autoware_perception_msgs::msg::PredictedObject obj; obj.kinematics.initial_pose_with_covariance.pose.position.x = 0.0; obj.kinematics.initial_pose_with_covariance.pose.position.y = 0.0; objs.objects.push_back(obj); diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index a5eb5fa295e2c..78c043b70bad3 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -161,7 +161,7 @@ If the target object is inside the road or crosswalk, this module outputs one or | Name | Type | Description | | ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | | `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | -| `~/output/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/output/objects` | `autoware_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | | `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | ## Parameters diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index d6ed0aa6c65b7..9b51d5fbbf0ee 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -90,11 +90,11 @@ struct PredictedRefPath using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 4da4f62be2ede..60da84a848b52 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -18,7 +18,7 @@ #include #include -#include +#include #include #include #include @@ -30,11 +30,11 @@ namespace map_based_prediction { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjectKinematics; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_perception_msgs::msg::TrackedObject; using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; using autoware_auto_perception_msgs::msg::TrackedObjects; diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 6a1354b37928f..a43f5dc0ea1fa 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -15,7 +15,7 @@ ament_cmake autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs interpolation lanelet2_extension motion_utils diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 8f555453b806a..1b72e9672a7d8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -799,9 +799,16 @@ PredictedObject MapBasedPredictionNode::convertToPredictedObject( { PredictedObject predicted_object; predicted_object.kinematics = convertToPredictedKinematics(tracked_object.kinematics); - predicted_object.classification = tracked_object.classification; + autoware_perception_msgs::msg::ObjectClassification classification; + for (size_t i = 0; i < tracked_object.classification.size(); i++) { + classification.label = tracked_object.classification[i].label; + classification.probability = tracked_object.classification[i].probability; + predicted_object.classification.push_back(classification); + } predicted_object.object_id = tracked_object.object_id; - predicted_object.shape = tracked_object.shape; + predicted_object.shape.type = tracked_object.shape.type; + predicted_object.shape.footprint = tracked_object.shape.footprint; + predicted_object.shape.dimensions = tracked_object.shape.dimensions; predicted_object.existence_probability = tracked_object.existence_probability; return predicted_object; diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index 395bc025c2821..b9c758e0872e1 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -29,7 +29,7 @@ See more details in the [models.md](models.md). | Name | Type | Description | | ------------- | ----------------------------------------------------- | ---------------- | | `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | -| `/vector/map` | `autoware_auto_msgs::msg::HADMapBin` | Map data | +| `/vector/map` | `autoware_map_msgs::msg::LaneletMapBin` | Map data | ### Output diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index cb781bebf90c9..a4bcd2d90c750 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -95,7 +95,7 @@ The current behavior tree structure is shown below. Each modules (LaneChange, Av | :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ | | ~/input/route | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | | ~/input/vector_map | `autoware_map_msgs::msg::LaneletMapBin` | map information. | -| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/objects | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | | ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | | ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 665f05318b6de..dca7f0de73461 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -23,7 +23,7 @@ #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include -#include +#include #include #include #include @@ -51,8 +51,8 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index ca58144464731..39be668f7df7a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -25,7 +25,7 @@ #include #include -#include +#include #include #include #include @@ -49,8 +49,8 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 100e8046f8d7f..985332abd6a90 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include #include @@ -32,7 +32,7 @@ namespace marker_utils::avoidance_marker { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidanceState; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 86f1a390261c6..21e2f41a78266 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -18,8 +18,8 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include -#include +#include +#include #include #include #include @@ -32,8 +32,8 @@ namespace marker_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index cbf40e59535be..b9a57a712a041 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -23,7 +23,7 @@ #include -#include +#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 077e0ecec0f29..11f573c13dfce 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include #include @@ -37,7 +37,7 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedPath; using tier4_autoware_utils::Polygon2d; struct MinMaxValue @@ -124,12 +124,12 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::string uuid{}; geometry_msgs::msg::Pose pose{}; - autoware_auto_perception_msgs::msg::Shape shape; + autoware_perception_msgs::msg::Shape shape; double vel{0.0}; double lat_vel{0.0}; bool is_object_on_ego_path{false}; std::optional latest_time_inside_ego_path{std::nullopt}; - std::vector predicted_paths{}; + std::vector predicted_paths{}; // NOTE: Previous values of the following are used for low-pass filtering. // Therefore, they has to be initialized as nullopt. @@ -319,7 +319,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface const std::vector & ego_path, const PredictedPath & obj_path) const; LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + const autoware_perception_msgs::msg::Shape & obj_shape) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( const std::vector & path_points_for_object_polygon, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c8b87c7b4759f..29fa7ecc0c69c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -38,8 +38,8 @@ namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Point2d; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp index 9591ac0582e04..193f4de093e41 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -50,7 +50,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects /// @return footprint polygons of the object's predicted paths MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion #endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 8da1521db6c28..4625f127b145d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include +#include #include #include #include @@ -26,7 +26,7 @@ namespace drivable_area_expansion { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index f4ab10db61b9c..d8092cfab4ba4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 062a84bcd5aef..0f850de3249e8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -20,8 +20,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -36,8 +36,8 @@ namespace behavior_path_planner { namespace goal_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index ac712fa2f0fa5..17759a037eec2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -23,7 +23,7 @@ #include -#include +#include #include #include #include @@ -35,9 +35,9 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index 67588ed0a67b6..f262015fa9bf5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -18,8 +18,8 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include -#include +#include +#include #include #include @@ -33,8 +33,8 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; /** diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp index 3634586c40540..3f64dac460ecb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -74,8 +74,8 @@ struct ExtendedPredictedObject geometry_msgs::msg::PoseWithCovariance initial_pose; geometry_msgs::msg::TwistWithCovariance initial_twist; geometry_msgs::msg::AccelWithCovariance initial_acceleration; - autoware_auto_perception_msgs::msg::Shape shape; - std::vector classification; + autoware_perception_msgs::msg::Shape shape; + std::vector classification; std::vector predicted_paths; }; using ExtendedPredictedObjects = std::vector; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp index b8bd1629d5f3f..363799604c1cd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -39,9 +39,9 @@ namespace behavior_path_planner::utils::path_safety_checker { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 7a973a3bd699f..a928c596f4fc6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -19,13 +19,13 @@ #include -#include +#include #include namespace behavior_path_planner { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index e7f51b2e86d5b..d2b9b555cf5f3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -21,8 +21,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -34,8 +34,8 @@ namespace behavior_path_planner::start_planner_utils { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 3f9591f6c2143..22b7bf7c7d59c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -22,10 +22,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include #include #include #include @@ -49,12 +49,12 @@ namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1f1eed3f95ec2..f7f40febe1045 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -45,7 +45,6 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_auto_vehicle_msgs diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index ef44bee663985..6e33caa4608ef 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -126,7 +126,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // route_handler auto qos_transient_local = rclcpp::QoS{1}.transient_local(); - vector_map_subscriber_ = create_subscription( + vector_map_subscriber_ = create_subscription( "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this)); route_subscriber_ = create_subscription( @@ -469,7 +469,7 @@ void BehaviorPathPlannerNode::run() } // check for map update - HADMapBin::ConstSharedPtr map_ptr{nullptr}; + LaneletMapBin::ConstSharedPtr map_ptr{nullptr}; { std::lock_guard lk_map(mutex_map_); // for has_received_map_ and map_ptr_ if (has_received_map_) { @@ -950,7 +950,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh planner_data_->traffic_light_id_map[signal.traffic_signal_id] = traffic_signal; } } -void BehaviorPathPlannerNode::onMap(const HADMapBin::ConstSharedPtr msg) +void BehaviorPathPlannerNode::onMap(const LaneletMapBin::ConstSharedPtr msg) { const std::lock_guard lock(mutex_map_); map_ptr_ = msg; diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index ce9cedc816a19..4125e386d08aa 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -56,7 +56,7 @@ MarkerArray createObjectsCubeMarkerArray( const ObjectDataArray & objects, std::string && ns, const Vector3 & scale, const ColorRGBA & color) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; MarkerArray msg; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a7604124ebe43..a7e50cc0cd933 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 6ed67fc817f28..0eed075ec9d2e 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -30,7 +30,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) : SceneModuleManagerInterface(node, name, config, {"left", "right"}) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; AvoidanceParameters p{}; @@ -328,7 +328,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( void AvoidanceModuleManager::updateModuleParams(const std::vector & parameters) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::updateParam; auto p = parameters_; diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index 7aba1d1b2d97f..b82a071a8813f 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -119,13 +119,13 @@ std::pair projectObstacleVelocityToTrajectory( return std::make_pair(projected_velocity[0], projected_velocity[1]); } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -343,7 +343,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::planWaitingApproval() bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; if (label == ObjectClassification::CAR && parameters_->avoid_car) { return true; @@ -769,7 +769,7 @@ std::pair DynamicAvoidanceModule DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & obj_shape) const + const autoware_perception_msgs::msg::Shape & obj_shape) const { const size_t obj_seg_idx = motion_utils::findNearestSegmentIndex(ego_path, obj_pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(obj_pose, obj_shape); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ee7b45de09e93..b9574f839a99b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -246,7 +246,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( : LaneChangeModuleManager( node, name, config, Direction::NONE, LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; rtc_interface_ptr_map_.clear(); diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp index 2e6d4929fdf02..a56edafe1c587 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp @@ -44,7 +44,7 @@ Polygon2d create_footprint(const geometry_msgs::msg::Pose & pose, const Polygon2 } MultiPolygon2d create_object_footprints( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, + const autoware_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params) { MultiPolygon2d footprints; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index fb2a96f1d5cbd..2eb0b31497460 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -45,8 +45,8 @@ namespace behavior_path_planner::utils::lane_change { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp index f6644a4f91e8f..d13230ae76d87 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp @@ -369,7 +369,7 @@ TargetObjectsOnLane createTargetObjectsOnLane( bool isTargetObjectType( const PredictedObject & object, const ObjectTypesToCheck & target_object_types) { - using autoware_auto_perception_msgs::msg::ObjectClassification; + using autoware_perception_msgs::msg::ObjectClassification; const auto t = utils::getHighestProbLabel(object.classification); return ( (t == ObjectClassification::CAR && target_object_types.check_car) || diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 19c408359e5f2..1e71a2379d5bf 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -134,8 +134,8 @@ bool isSamePoint(const geometry_msgs::msg::Point & point1, const geometry_msgs:: namespace behavior_path_planner::utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::Shape; using geometry_msgs::msg::PoseWithCovarianceStamped; using tf2::fromMsg; using tier4_autoware_utils::Point2d; diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index a5fc19d1ecbbe..d83be883dbea1 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -27,7 +27,7 @@ constexpr double epsilon = 1e-6; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::Shape; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; @@ -143,7 +143,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) obj_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(0.0); Shape shape; - shape.type = autoware_auto_perception_msgs::msg::Shape::POLYGON; + shape.type = autoware_perception_msgs::msg::Shape::POLYGON; shape.footprint.points.resize(5); shape.footprint.points.at(0).x = 3.0; shape.footprint.points.at(0).y = 0.0; diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index 9485f165849f2..d862bea16d106 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 775f3b1c65973..013ab2e0cf5b8 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -299,7 +299,7 @@ bool BlindSpotModule::generateStopLine( } void BlindSpotModule::cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const + autoware_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const { const rclcpp::Time current_time = clock_->now(); @@ -372,7 +372,7 @@ int BlindSpotModule::insertPoint( bool BlindSpotModule::checkObstacleInBlindSpot( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const { /* get detection area */ @@ -387,7 +387,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( debug_data_.detection_areas_for_blind_spot = areas_opt.get().detection_areas; debug_data_.conflict_areas_for_blind_spot = areas_opt.get().conflict_areas; - autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; + autoware_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); // check objects in blind spot areas @@ -419,7 +419,7 @@ bool BlindSpotModule::checkObstacleInBlindSpot( } bool BlindSpotModule::isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const { const auto ego_yaw = tf2::getYaw(ego_pose.orientation); @@ -614,15 +614,15 @@ lanelet::LineString2d BlindSpotModule::getVehicleEdge( } bool BlindSpotModule::isTargetObjectType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE || + autoware_perception_msgs::msg::ObjectClassification::BICYCLE || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN || + autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index e738fb2e7618a..21ee6332a9d60 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include #include #include @@ -54,7 +54,7 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose judge_point_pose; std::vector conflict_areas_for_blind_spot; std::vector detection_areas_for_blind_spot; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; }; public: @@ -108,7 +108,7 @@ class BlindSpotModule : public SceneModuleInterface lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const int closest_idx, const geometry_msgs::msg::Pose & stop_line_pose) const; /** @@ -150,7 +150,7 @@ class BlindSpotModule : public SceneModuleInterface * @param object Dynamic object * @return True when object belong to targeted classes */ - bool isTargetObjectType(const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + bool isTargetObjectType(const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Check if at least one of object's predicted position is in area @@ -159,7 +159,7 @@ class BlindSpotModule : public SceneModuleInterface * @return True when at least one of object's predicted position is in area */ bool isPredictedPathInArea( - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, const std::vector & areas, geometry_msgs::msg::Pose ego_pose) const; /** @@ -216,7 +216,7 @@ class BlindSpotModule : public SceneModuleInterface * @param time_thr time threshold to cut path */ void cutPredictPathWithDuration( - autoware_auto_perception_msgs::msg::PredictedObjects * objects_ptr, + autoware_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 3830aa9edddff..aab9d2118cfff 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -19,7 +19,6 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 autoware_perception_msgs diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 0768101179857..a2a05bad00efa 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include @@ -49,9 +49,9 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::TrafficSignalElement; using lanelet::autoware::Crosswalk; diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 47bbef7111da0..a5090c7bf276b 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml index 0c9b3407d0f38..f8699a7bd82c0 100644 --- a/planning/behavior_velocity_intersection_module/package.xml +++ b/planning/behavior_velocity_intersection_module/package.xml @@ -18,7 +18,6 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs autoware_auto_planning_msgs autoware_perception_msgs behavior_velocity_planner_common diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 476238f6b1e9e..50e4fdd1f1ce1 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -49,7 +49,7 @@ namespace { Polygon2d createOneStepPolygon( const geometry_msgs::msg::Pose & prev_pose, const geometry_msgs::msg::Pose & next_pose, - const autoware_auto_perception_msgs::msg::Shape & shape) + const autoware_perception_msgs::msg::Shape & shape) { const auto prev_poly = tier4_autoware_utils::toPolygon2d(prev_pose, shape); const auto next_poly = tier4_autoware_utils::toPolygon2d(next_pose, shape); @@ -72,21 +72,21 @@ Polygon2d createOneStepPolygon( } // namespace static bool isTargetCollisionVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE || + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE) { + autoware_perception_msgs::msg::ObjectClassification::BICYCLE) { return true; } return false; @@ -2065,7 +2065,7 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel) { const auto & object_pose = object.kinematics.initial_pose_with_covariance.pose; @@ -2120,7 +2120,7 @@ IntersectionModule::OcclusionType IntersectionModule::getOcclusionStatus( stopping_point.y += lat_offset * std::sin(lane_yaw + M_PI / 2.0); // calculate footprint of predicted stopping pose - autoware_auto_perception_msgs::msg::PredictedObject predicted_object = object; + autoware_perception_msgs::msg::PredictedObject predicted_object = object; predicted_object.kinematics.initial_pose_with_covariance.pose.position = stopping_point; predicted_object.kinematics.initial_pose_with_covariance.pose.orientation = tier4_autoware_utils::createQuaternionFromRPY(0, 0, lane_yaw); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c794ef6c53aad..349d621a0c904 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -362,7 +362,7 @@ class IntersectionModule : public SceneModuleInterface bool IntersectionModule::checkFrontVehicleDeceleration( lanelet::ConstLanelets & ego_lane_with_next_lane, lanelet::ConstLanelet & closest_lanelet, const Polygon2d & stuck_vehicle_detect_area, - const autoware_auto_perception_msgs::msg::PredictedObject & object, + const autoware_perception_msgs::msg::PredictedObject & object, const double assumed_front_car_decel); */ diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 19e7b9201093f..c1ffd910be863 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 093f90e4b3df7..ee2fcadc4209d 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1116,7 +1116,7 @@ std::optional generateInterpolatedPath( // from here geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state) + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state) { if (obj_state.initial_twist_with_covariance.twist.linear.x >= 0) { return obj_state.initial_pose_with_covariance.pose; @@ -1133,26 +1133,26 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( } static bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) + const autoware_perception_msgs::msg::PredictedObject & object) { if ( object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.at(0).label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; } bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data) { @@ -1179,7 +1179,7 @@ bool checkStuckVehicleInIntersection( } bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data) { diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 0ff35ab5c0c23..dd9ca3e313718 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -122,15 +122,15 @@ std::optional generateInterpolatedPath( const rclcpp::Logger logger); geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( - const autoware_auto_perception_msgs::msg::PredictedObjectKinematics & obj_state); + const autoware_perception_msgs::msg::PredictedObjectKinematics & obj_state); bool checkStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const Polygon2d & stuck_vehicle_detect_area, const double stuck_vehicle_vel_thr, DebugData * debug_data); bool checkYieldStuckVehicleInIntersection( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_ptr, const lanelet::BasicPolygon2d & ego_poly, const lanelet::CompoundPolygon3d & first_attention_area, const double stuck_vehicle_vel_thr, const double yield_stuck_distance_thr, DebugData * debug_data); diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index d1ee4c2f79410..130ae23c0ddc4 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -47,15 +47,15 @@ struct DebugData std::optional stuck_vehicle_detect_area{std::nullopt}; std::optional candidate_collision_ego_lane_polygon{std::nullopt}; std::vector candidate_collision_object_polygons; - autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; - autoware_auto_perception_msgs::msg::PredictedObjects amber_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; - autoware_auto_perception_msgs::msg::PredictedObjects stuck_targets; - autoware_auto_perception_msgs::msg::PredictedObjects yield_stuck_targets; + autoware_perception_msgs::msg::PredictedObjects conflicting_targets; + autoware_perception_msgs::msg::PredictedObjects amber_ignore_targets; + autoware_perception_msgs::msg::PredictedObjects red_overshoot_ignore_targets; + autoware_perception_msgs::msg::PredictedObjects stuck_targets; + autoware_perception_msgs::msg::PredictedObjects yield_stuck_targets; std::vector occlusion_polygons; std::optional> nearest_occlusion_projection{std::nullopt}; - autoware_auto_perception_msgs::msg::PredictedObjects blocking_attention_objects; + autoware_perception_msgs::msg::PredictedObjects blocking_attention_objects; std::optional absence_traffic_light_creep_wall{std::nullopt}; std::optional static_occlusion_with_traffic_light_timeout{std::nullopt}; }; @@ -180,7 +180,7 @@ struct PathLanelets struct TargetObject { - autoware_auto_perception_msgs::msg::PredictedObject object; + autoware_perception_msgs::msg::PredictedObject object; std::optional attention_lanelet{std::nullopt}; std::optional stop_line{std::nullopt}; std::optional dist_to_stop_line{std::nullopt}; diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index a9c21f1aba189..6c710b581c8e0 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ceef50094e374..dc5000707bdaa 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -220,7 +220,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects @@ -348,19 +348,19 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( } bool NoStoppingAreaModule::isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const + const autoware_perception_msgs::msg::PredictedObject & object) const { if ( object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::CAR || + autoware_perception_msgs::msg::ObjectClassification::CAR || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::BUS || + autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK || + autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER || + autoware_perception_msgs::msg::ObjectClassification::TRAILER || object.classification.front().label == - autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { + autoware_perception_msgs::msg::ObjectClassification::MOTORCYCLE) { return true; } return false; diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 84f260e5e2c2b..008c888067564 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -24,9 +24,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -101,7 +101,7 @@ class NoStoppingAreaModule : public SceneModuleInterface * @return true if the object has a target type */ bool isTargetStuckVehicleType( - const autoware_auto_perception_msgs::msg::PredictedObject & object) const; + const autoware_perception_msgs::msg::PredictedObject & object) const; /** * @brief Check if there is a stopped vehicle in stuck vehicle detect area. @@ -111,7 +111,7 @@ class NoStoppingAreaModule : public SceneModuleInterface */ bool checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); /** diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index cb5bd744edfa8..f722177d607b1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,7 +16,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 849e16eb0834c..d7c72288903a3 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -25,9 +25,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -55,8 +55,8 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; namespace grid_utils { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::TransformStamped; using nav_msgs::msg::MapMetaData; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index c153a727b8a7e..b64a5023e86d7 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,8 +23,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index cc07461864c16..0c131225dbd27 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -23,9 +23,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include #include @@ -50,9 +50,9 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index b7278aff29902..daabbd4982e8a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -42,7 +42,7 @@ namespace { namespace utils = behavior_velocity_planner::occlusion_spot_utils; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) { diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 2cd7ea3582452..7d3ec5b908300 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 85ae85f1895d9..01347b293be36 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -24,8 +24,8 @@ using Point = geometry_msgs::msg::Point; using Vector3 = geometry_msgs::msg::Vector3; -using DynamicObjects = autoware_auto_perception_msgs::msg::PredictedObjects; -using DynamicObject = autoware_auto_perception_msgs::msg::PredictedObject; +using DynamicObjects = autoware_perception_msgs::msg::PredictedObjects; +using DynamicObject = autoware_perception_msgs::msg::PredictedObject; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index 43dea2f2153ff..089672b517d1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,7 +14,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common geometry_msgs diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 78d9b73c86a17..6037eabb5b35a 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -62,7 +62,7 @@ bool object_is_incoming( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const rclcpp::Logger & logger) { // skip the dynamic object if it is not in a lane preceding the overlapped lane @@ -144,7 +144,7 @@ std::optional> object_time_to_range( } std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger) { const auto & p = object.kinematics.initial_pose_with_covariance.pose.position; diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 2898595c2e74c..0058673295ff3 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include @@ -63,7 +63,7 @@ double time_along_path(const EgoData & ego_data, const size_t target_idx); /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const std::shared_ptr route_handler, const rclcpp::Logger & logger); /// @brief use the lanelet map to estimate the times when an object will reach the enter and exit /// points of an overlapping range @@ -75,7 +75,7 @@ std::optional> object_time_to_range( /// @return an optional pair (time at enter [s], time at exit [s]). If the dynamic object drives in /// the opposite direction, time at enter > time at exit. std::optional> object_time_to_range( - const autoware_auto_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, + const autoware_perception_msgs::msg::PredictedObject & object, const OverlapRange & range, const DecisionInputs & inputs, const rclcpp::Logger & logger); /// @brief decide whether an object is coming in the range at the same time as ego /// @details the condition depends on the mode (threshold, intervals, ttc) diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index 13bad0d047922..ea9e9504cb175 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -28,16 +28,16 @@ namespace behavior_velocity_planner::out_of_lane /// @param [in] ego_data ego data /// @param [in] params parameters /// @return filtered predicted objects -autoware_auto_perception_msgs::msg::PredictedObjects filter_predicted_objects( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, +autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( + const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params) { - autoware_auto_perception_msgs::msg::PredictedObjects filtered_objects; + autoware_perception_msgs::msg::PredictedObjects filtered_objects; filtered_objects.header = objects.header; for (const auto & object : objects.objects) { const auto is_pedestrian = std::find_if(object.classification.begin(), object.classification.end(), [](const auto & c) { - return c.label == autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN; + return c.label == autoware_perception_msgs::msg::ObjectClassification::PEDESTRIAN; }) != object.classification.end(); if (is_pedestrian) continue; diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index c8ef397913c8f..0f32578ae74a5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -22,8 +22,8 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index cdf8eef8f277b..bcd22d03d6e03 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 64d88f15e6966..8cf22e78cece5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -147,7 +147,7 @@ struct DecisionInputs { OverlapRanges ranges{}; EgoData ego_data; - autoware_auto_perception_msgs::msg::PredictedObjects objects{}; + autoware_perception_msgs::msg::PredictedObjects objects{}; std::shared_ptr route_handler{}; lanelet::ConstLanelets lanelets{}; }; diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index cad47c16c1452..3017e65e4b2ec 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -34,7 +34,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | | `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | | `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | | `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | | `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 62cd85b4f469e..1df100c666590 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -34,7 +34,7 @@ rosidl_default_generators - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_map_msgs behavior_velocity_planner_common diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ad740da68e93a..455fb7822cf40 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -85,7 +85,7 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio // Subscribers sub_predicted_objects_ = - this->create_subscription( + this->create_subscription( "~/input/dynamic_objects", 1, std::bind(&BehaviorVelocityPlannerNode::onPredictedObjects, this, _1), createSubscriptionOptions(this)); @@ -224,7 +224,7 @@ void BehaviorVelocityPlannerNode::onOccupancyGrid( } void BehaviorVelocityPlannerNode::onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { std::lock_guard lock(mutex_); planner_data_.predicted_objects = msg; diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index 7599ff84561d1..cc7786523ce29 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -23,7 +23,7 @@ #include #include -#include +#include #include #include #include @@ -63,7 +63,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node // subscriber rclcpp::Subscription::SharedPtr trigger_sub_path_with_lane_id_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_predicted_objects_; rclcpp::Subscription::SharedPtr sub_no_ground_pointcloud_; rclcpp::Subscription::SharedPtr sub_vehicle_odometry_; @@ -79,7 +79,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node void onTrigger( const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg); void onPredictedObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); void onNoGroundPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); void onAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg); diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index c75807ab926bb..6d45b009e30ad 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 23f3b32060aa2..25336603ab676 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -67,7 +67,7 @@ struct PlannerData geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_acceleration; static constexpr double velocity_buffer_time_sec = 10.0; std::deque velocity_buffer; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr predicted_objects; pcl::PointCloud::ConstPtr no_ground_pointcloud; // occupancy grid nav_msgs::msg::OccupancyGrid::ConstSharedPtr occupancy_grid; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index 98df29d1c44c0..15db7457382d5 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -39,7 +39,7 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( const int64_t lane_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b); visualization_msgs::msg::MarkerArray createPointsMarkerArray( diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 86273b1e35e1d..bc308434f74cf 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -63,7 +63,7 @@ using LineString2d = tier4_autoware_utils::LineString2d; using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_planning_msgs::msg::StopFactor; diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp index 6ff2ef53176c4..914ce306c644f 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/behavior_velocity_planner_common/src/utilization/debug.cpp @@ -83,7 +83,7 @@ visualization_msgs::msg::MarkerArray createPathMarkerArray( } visualization_msgs::msg::MarkerArray createObjectsMarkerArray( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, + const autoware_perception_msgs::msg::PredictedObjects & objects, const std::string & ns, const int64_t module_id, const rclcpp::Time & now, const double r, const double g, const double b) { visualization_msgs::msg::MarkerArray msg; diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index a36cfdf6485c6..345fbb55681a0 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -74,8 +74,8 @@ Abstracted obstacle data has following information. | Name | Type | Description | | ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | | pose | `geometry_msgs::msg::Pose` | pose of the obstacle | -| classifications | `std::vector` | classifications with probability | -| shape | `autoware_auto_perception_msgs::msg::Shape` | shape of the obstacle | +| classifications | `std::vector` | classifications with probability | +| shape | `autoware_perception_msgs::msg::Shape` | shape of the obstacle | | predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | | min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | | max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index fb3c49bb750a6..8fac6be06686f 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -17,7 +17,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 5217193c9673e..3c4731aba8560 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -43,9 +43,9 @@ namespace behavior_velocity_planner { -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using run_out_utils::DynamicObstacle; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index 925d8ea8b234c..fd42805e2f40c 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -29,7 +29,7 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; using run_out_utils::PlannerParam; diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/behavior_velocity_run_out_module/src/utils.cpp index 61396da250c8d..81d3d5e2eac54 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/behavior_velocity_run_out_module/src/utils.cpp @@ -54,7 +54,7 @@ Polygon2d createBoostPolyFromMsg(const std::vector & std::uint8_t getHighestProbLabel(const std::vector & classification) { - std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + std::uint8_t label = autoware_perception_msgs::msg::ObjectClassification::UNKNOWN; float highest_prob = 0.0; for (const auto & _class : classification) { if (highest_prob < _class.probability) { diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index b25a8fc94bff3..ff2c6cabe5e3d 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -19,7 +19,7 @@ #include #include -#include +#include #include #include @@ -31,9 +31,9 @@ namespace behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Box2d; diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index f8a1b4933d9bd..74dd462be275a 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -8,7 +8,7 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan | Name | Type | Description | | ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- | -| `~input/objects` | autoware_auto_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | +| `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | | `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | | `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | | `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index e46500259dc93..0b21a2e5b74f9 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -53,7 +53,7 @@ #include #include -#include +#include #include #include @@ -84,7 +84,7 @@ class CostmapGenerator : public rclcpp::Node bool use_parkinglot_; lanelet::LaneletMapPtr lanelet_map_; - autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; + autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; sensor_msgs::msg::PointCloud2::ConstSharedPtr points_; std::string costmap_frame_; @@ -114,7 +114,7 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; rclcpp::Subscription::SharedPtr sub_scenario_; @@ -148,7 +148,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief callback for DynamicObjectArray /// \param[in] in_objects input DynamicObjectArray usually from prediction or perception /// component - void onObjects(const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); + void onObjects(const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg); /// \brief callback for sensor_msgs::PointCloud2 /// \param[in] in_points input sensor_msgs::PointCloud2. Assuming ground-filtered pointcloud @@ -190,7 +190,7 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost from DynamicObjectArray /// \param[in] in_objects: subscribed DynamicObjectArray grid_map::Matrix generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map grid_map::Matrix generatePrimitivesCostmap(); diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp index 84a7807e432cd..a88bb97a623e2 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp @@ -53,7 +53,7 @@ #include #endif -#include +#include #include @@ -71,7 +71,7 @@ class ObjectsToCostmap grid_map::Matrix makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); private: const int NUMBER_OF_POINTS; @@ -84,7 +84,7 @@ class ObjectsToCostmap /// \param[in] expand_rectangle_size: expanding 4 points /// \param[out] 4 rectangle points Eigen::MatrixXd makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make polygon(grid_map::Polygon) from 4 rectangle's points @@ -93,7 +93,7 @@ class ObjectsToCostmap /// \param[out] polygon with 4 rectangle points grid_map::Polygon makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size); /// \brief make expanded point from convex hull's point @@ -111,7 +111,7 @@ class ObjectsToCostmap /// \param[out] polygon object with convex hull points grid_map::Polygon makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size); /// \brief set cost in polygon by using DynamicObject's score diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index a82d8000ad077..3c25b84772586 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -196,7 +196,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) // Subscribers using std::placeholders::_1; - sub_objects_ = this->create_subscription( + sub_objects_ = this->create_subscription( "~/input/objects", 1, std::bind(&CostmapGenerator::onObjects, this, _1)); sub_points_ = this->create_subscription( "~/input/points_no_ground", rclcpp::SensorDataQoS(), @@ -278,7 +278,7 @@ void CostmapGenerator::onLaneletMapBin( } void CostmapGenerator::onObjects( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr msg) { objects_ = msg; } @@ -388,12 +388,12 @@ grid_map::Matrix CostmapGenerator::generatePointsCostmap( return points_costmap; } -autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( +autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformObjects( const tf2_ros::Buffer & tf_buffer, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects, const std::string & target_frame_id, const std::string & src_frame_id) { - auto objects = new autoware_auto_perception_msgs::msg::PredictedObjects(); + auto objects = new autoware_perception_msgs::msg::PredictedObjects(); *objects = *in_objects; objects->header.frame_id = target_frame_id; @@ -411,11 +411,11 @@ autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr transformOb object.kinematics.initial_pose_with_covariance.pose = output_stamped.pose; } - return autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); + return autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr(objects); } grid_map::Matrix CostmapGenerator::generateObjectsCostmap( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { const auto object_frame = in_objects->header.frame_id; const auto transformed_objects = diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp index 79ab4a3bf66c1..f6f024fb92a4e 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp @@ -59,7 +59,7 @@ ObjectsToCostmap::ObjectsToCostmap() } Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { double length = in_object.shape.dimensions.x + expand_rectangle_size; @@ -85,7 +85,7 @@ Eigen::MatrixXd ObjectsToCostmap::makeRectanglePoints( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectBox( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_rectangle_size) { grid_map::Polygon polygon; @@ -122,7 +122,7 @@ geometry_msgs::msg::Point ObjectsToCostmap::makeExpandedPoint( grid_map::Polygon ObjectsToCostmap::makePolygonFromObjectConvexHull( const std_msgs::msg::Header & header, - const autoware_auto_perception_msgs::msg::PredictedObject & in_object, + const autoware_perception_msgs::msg::PredictedObject & in_object, const double expand_polygon_size) { grid_map::Polygon polygon; @@ -157,7 +157,7 @@ void ObjectsToCostmap::setCostInPolygon( grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( const grid_map::GridMap & costmap, const double expand_polygon_size, const double size_of_expansion_kernel, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects) { grid_map::GridMap objects_costmap = costmap; objects_costmap.add(OBJECTS_COSTMAP_LAYER_, 0); @@ -165,11 +165,11 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( for (const auto & object : in_objects->objects) { grid_map::Polygon polygon; - if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + if (object.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { polygon = makePolygonFromObjectConvexHull(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); - } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // TODO(Kenji Miyake): Add makePolygonFromObjectCylinder polygon = makePolygonFromObjectBox(in_objects->header, object, expand_polygon_size); } diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index 0015787e25d62..aa01572c9a352 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -15,7 +15,7 @@ ament_cmake_auto autoware_cmake - autoware_auto_perception_msgs + autoware_perception_msgs autoware_map_msgs grid_map_ros lanelet2_extension diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/costmap_generator/test/test_objects_to_costmap.cpp index 667ee70ef53ff..15b853d0782aa 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/costmap_generator/test/test_objects_to_costmap.cpp @@ -17,7 +17,7 @@ #include -using LABEL = autoware_auto_perception_msgs::msg::ObjectClassification; +using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test { @@ -69,10 +69,10 @@ grid_y */ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) { - auto objs = std::make_shared(); - autoware_auto_perception_msgs::msg::PredictedObject object; + auto objs = std::make_shared(); + autoware_perception_msgs::msg::PredictedObject object; - object.classification.push_back(autoware_auto_perception_msgs::msg::ObjectClassification{}); + object.classification.push_back(autoware_perception_msgs::msg::ObjectClassification{}); object.classification.at(0).label = LABEL::CAR; object.classification.at(0).probability = 0.8; @@ -85,7 +85,7 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) object.kinematics.initial_pose_with_covariance.pose.orientation.z = 0; object.kinematics.initial_pose_with_covariance.pose.orientation.w = 1; - object.shape.type = autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX; + object.shape.type = autoware_perception_msgs::msg::Shape::BOUNDING_BOX; object.shape.dimensions.x = 5; object.shape.dimensions.y = 3; object.shape.dimensions.z = 2; diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 532761b1f0cb7..ec07fab7b0511 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -18,7 +18,7 @@ The `obstacle_cruise_planner` package has following modules. | Name | Type | Description | | -------------------- | ----------------------------------------------- | ---------------- | | `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | | `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index 0d0a2890f4fa8..2a10ddbcd9da1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -18,8 +18,8 @@ #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" @@ -36,11 +36,11 @@ using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::AccelStamped; diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index e724ddb3e6bd7..5a839db4771b5 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -17,7 +17,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs geometry_msgs interpolation diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index 99e4217d0c9ff..c3c101a8e4994 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -19,7 +19,7 @@ | `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | | `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | | `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | -| `~/input/dynamic_objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | | `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index 9d95c5a4796d4..d7bef2ea4a05b 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -32,7 +32,7 @@ namespace motion_planning { using autoware_auto_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; class AdaptiveCruiseController { public: @@ -44,7 +44,7 @@ class AdaptiveCruiseController const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_velocity_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header); @@ -191,7 +191,7 @@ class AdaptiveCruiseController const std_msgs::msg::Header & trajectory_header); double calcTrajYaw(const TrajectoryPoints & trajectory, const int collision_point_idx); bool estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity); bool estimatePointVelocityFromPcl( const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 368ad34b9c4e2..821f912c0d715 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include #include @@ -75,7 +75,7 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using tier4_autoware_utils::Point2d; @@ -92,7 +92,7 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; struct ObstacleWithDetectionTime { explicit ObstacleWithDetectionTime(const rclcpp::Time & t, pcl::PointXYZ & p) diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index 6b8e2b8b3ec9d..c986b25d62e35 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include #include @@ -256,7 +256,7 @@ struct PlannerData pcl::PointXYZ lateral_nearest_slow_down_point; - autoware_auto_perception_msgs::msg::Shape slow_down_object_shape{}; + autoware_perception_msgs::msg::Shape slow_down_object_shape{}; rclcpp::Time nearest_collision_point_time{}; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 7e7e659325abf..01eddc35d5dd2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -55,8 +55,8 @@ using vehicle_info_util::VehicleInfo; using TrajectoryPoints = std::vector; using PointCloud = pcl::PointCloud; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using PointVariant = boost::variant; boost::optional> calcFeasibleMarginAndVelocity( @@ -114,10 +114,10 @@ void getLateralNearestPointForPredictedObject( Pose getVehicleCenterFromBase(const Pose & base_pose, const VehicleInfo & vehicle_info); Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape); + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape); Polygon2d convertBoundingBoxObjectToGeometryPolygon( const Pose & current_pose, const double & base_to_front, const double & base_to_rear, @@ -148,7 +148,7 @@ boost::optional getObstacleFromUuid( bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos); -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape); +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape); rclcpp::SubscriptionOptions createSubscriptionOptions(rclcpp::Node * node_ptr); diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 36ffca308981f..4ff8c4019844f 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -20,7 +20,7 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs diagnostic_msgs motion_utils diff --git a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp index c0f57489078d6..d87f44e329655 100644 --- a/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp +++ b/planning/obstacle_stop_planner/src/adaptive_cruise_control.cpp @@ -198,7 +198,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( const TrajectoryPoints & trajectory, const int nearest_collision_point_idx, const geometry_msgs::msg::Pose self_pose, const pcl::PointXYZ & nearest_collision_point, const rclcpp::Time nearest_collision_point_time, - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr, bool * need_to_stop, TrajectoryPoints * output_trajectory, const std_msgs::msg::Header trajectory_header) { @@ -313,7 +313,7 @@ void AdaptiveCruiseController::insertAdaptiveCruiseVelocity( calcUpperVelocity(col_point_distance, point_velocity, current_velocity); pub_debug_->publish(debug_values_); - if (target_object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (target_object.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { // if the target object is obstacle return stop true RCLCPP_DEBUG_THROTTLE( node_->get_logger(), *node_->get_clock(), std::chrono::milliseconds(1000).count(), @@ -406,7 +406,7 @@ double AdaptiveCruiseController::calcTrajYaw( } bool AdaptiveCruiseController::estimatePointVelocityFromObject( - const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr object_ptr, const double traj_yaw, const pcl::PointXYZ & nearest_collision_point, double * velocity) { geometry_msgs::msg::Point nearest_collision_p_ros; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 9a9712c5a2503..59542afdb3ef5 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -40,7 +40,7 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObject; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcLongitudinalOffsetToSegment; using motion_utils::calcSignedArcLength; @@ -625,7 +625,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_range; bool found_slow_down_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -633,7 +633,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( slow_down_param_.pedestrian_lateral_margin); found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.vehicle_lateral_margin); @@ -644,7 +644,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( found_slow_down_object = bg::intersects(one_step_move_slow_down_range, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_range, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -719,15 +719,15 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_slow_down_vehicle_polygon; const auto & obj = filtered_objects.objects.at(nearest_slow_down_object_index); - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.pedestrian_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.vehicle_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_slow_down_vehicle_polygon, vehicle_info, slow_down_param_.unknown_lateral_margin); @@ -764,7 +764,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d one_step_move_collision_polygon; bool found_collision_object = false; Polygon2d object_polygon{}; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); @@ -773,7 +773,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.pedestrian_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { const double & length_m = obj.shape.dimensions.x / 2; const double & width_m = obj.shape.dimensions.y / 2; object_polygon = convertBoundingBoxObjectToGeometryPolygon( @@ -783,7 +783,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( stop_param.vehicle_lateral_margin); found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { object_polygon = convertPolygonObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); createOneStepPolygon( @@ -880,17 +880,17 @@ void ObstacleStopPlannerNode::searchPredictedObject( collision_point.y() = predicted_object_history_.at(j).point.y; Polygon2d one_step_move_vehicle_polygon; // create one step polygon for vehicle - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.vehicle_lateral_margin); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.unknown_lateral_margin); @@ -928,13 +928,13 @@ void ObstacleStopPlannerNode::searchPredictedObject( Polygon2d object_polygon{}; const auto & obj = predicted_object_history_.at(nearest_collision_object_index).object; - if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + if (obj.shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.pedestrian_lateral_margin); object_polygon = convertCylindricalObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, obj.shape); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.vehicle_lateral_margin); @@ -943,7 +943,7 @@ void ObstacleStopPlannerNode::searchPredictedObject( object_polygon = convertBoundingBoxObjectToGeometryPolygon( obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m); - } else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (obj.shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { createOneStepPolygon( p_front, p_back, one_step_move_vehicle_polygon, vehicle_info, stop_param.unknown_lateral_margin); @@ -1133,7 +1133,7 @@ void ObstacleStopPlannerNode::insertVelocity( if (node_param_.use_predicted_objects) { if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + autoware_perception_msgs::msg::Shape::CYLINDER) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1141,7 +1141,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.pedestrian_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * @@ -1149,7 +1149,7 @@ void ObstacleStopPlannerNode::insertVelocity( slow_down_param_.vehicle_lateral_margin; } else if ( planner_data.slow_down_object_shape.type == - autoware_auto_perception_msgs::msg::Shape::POLYGON) { + autoware_perception_msgs::msg::Shape::POLYGON) { slow_down_velocity = slow_down_param_.min_slow_down_velocity + (slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) * diff --git a/planning/obstacle_stop_planner/src/planner_utils.cpp b/planning/obstacle_stop_planner/src/planner_utils.cpp index be0ef13626f92..0c202a0cc2c88 100644 --- a/planning/obstacle_stop_planner/src/planner_utils.cpp +++ b/planning/obstacle_stop_planner/src/planner_utils.cpp @@ -32,8 +32,8 @@ namespace motion_planning { -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using motion_utils::calcDecelDistWithJerkAndAccConstraints; using motion_utils::findFirstNearestIndexWithSoftConstraints; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; @@ -630,7 +630,7 @@ Polygon2d convertBoundingBoxObjectToGeometryPolygon( } Polygon2d convertCylindricalObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; @@ -653,7 +653,7 @@ Polygon2d convertCylindricalObjectToGeometryPolygon( } Polygon2d convertPolygonObjectToGeometryPolygon( - const Pose & current_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) + const Pose & current_pose, const autoware_perception_msgs::msg::Shape & obj_shape) { Polygon2d object_polygon; tf2::Transform tf_map2obj; @@ -696,13 +696,13 @@ bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & ob return base_pose_vec.dot(obstacle_vec) >= 0; } -double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObstacleMaxLength(const autoware_perception_msgs::msg::Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == autoware_perception_msgs::msg::Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == autoware_perception_msgs::msg::Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 16b1cb60b853f..7d30532e3a8be 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -151,7 +151,7 @@ For example a value of `1` means all trajectory points will be evaluated while a | `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | | `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | | `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | -| `~/input/dynamic_obstacles` | `autoware_auto_perception_msgs/PredictedObjects` | Dynamic objects | +| `~/input/dynamic_obstacles` | `autoware_perception_msgs/PredictedObjects` | Dynamic objects | | `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | | `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 4d89d4b440c21..50d0051a6eb20 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -19,7 +19,7 @@ #include "obstacle_velocity_limiter/parameters.hpp" #include "obstacle_velocity_limiter/types.hpp" -#include +#include #include #include @@ -63,7 +63,7 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel); /// @brief create footprint polygons from projection lines diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 7dcb86f463dd6..6c882dc074b55 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -24,7 +24,7 @@ #include #include -#include +#include #include #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp index 8620c254968fe..618437edc981a 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacles.hpp @@ -20,7 +20,7 @@ #include -#include +#include #include #include @@ -163,7 +163,7 @@ polygon_t createObjectPolygon( /// @param [in] min_velocity objects with velocity lower will be ignored /// @return polygons of the objects multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity); /// @brief add obstacles obtained from sensors to the given Obstacles object diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index e44011e1f0ee3..8aeec6262a8a3 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include +#include #include #include #include @@ -25,7 +25,7 @@ namespace obstacle_velocity_limiter { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using nav_msgs::msg::OccupancyGrid; diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 377f061faffd7..3171be9ffa3ff 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -11,7 +11,7 @@ autoware_cmake eigen3_cmake_module - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs eigen grid_map_msgs diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index ac1b4bde65211..46d024990a402 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -35,7 +35,7 @@ Float calculateSafeVelocity( } multi_polygon_t createPolygonMasks( - const autoware_auto_perception_msgs::msg::PredictedObjects & dynamic_obstacles, + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, const Float min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); diff --git a/planning/obstacle_velocity_limiter/src/obstacles.cpp b/planning/obstacle_velocity_limiter/src/obstacles.cpp index 603e573e68f17..5d4c1d29665df 100644 --- a/planning/obstacle_velocity_limiter/src/obstacles.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacles.cpp @@ -59,7 +59,7 @@ polygon_t createObjectPolygon( } multi_polygon_t createObjectPolygons( - const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const double buffer, + const autoware_perception_msgs::msg::PredictedObjects & objects, const double buffer, const double min_velocity) { multi_polygon_t polygons; diff --git a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp index c5e69e5fd8fb9..d4f44e7a6a775 100644 --- a/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp +++ b/planning/obstacle_velocity_limiter/test/test_collision_distance.cpp @@ -17,8 +17,8 @@ #include "obstacle_velocity_limiter/types.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -384,8 +384,8 @@ TEST(TestCollisionDistance, arcDistance) TEST(TestCollisionDistance, createObjPolygons) { - using autoware_auto_perception_msgs::msg::PredictedObject; - using autoware_auto_perception_msgs::msg::PredictedObjects; + using autoware_perception_msgs::msg::PredictedObject; + using autoware_perception_msgs::msg::PredictedObjects; using obstacle_velocity_limiter::createObjectPolygons; PredictedObjects objects; diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 7bf54c0278a27..77140458dea9d 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -19,10 +19,10 @@ from subprocess import check_output import time -from autoware_auto_perception_msgs.msg import DetectedObjects -from autoware_auto_perception_msgs.msg import PredictedObjects -from autoware_auto_perception_msgs.msg import TrackedObjects -from autoware_auto_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray +from autoware_perception_msgs.msg import DetectedObjects +from autoware_perception_msgs.msg import PredictedObjects +from autoware_perception_msgs.msg import TrackedObjects +from autoware_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray from autoware_perception_msgs.msg import TrafficSignalArray from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry @@ -132,7 +132,7 @@ def load_rosbag(self, rosbag2_path: str): if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) self.is_auto_traffic_signals = ( - "autoware_auto_perception_msgs" in type(msg).__module__ + "autoware_perception_msgs" in type(msg).__module__ ) def kill_online_perception_node(self): diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index 9acd3d47335d1..8e8ca89637b09 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -36,7 +36,7 @@ #include #include -#include +#include #include #include #include @@ -75,7 +75,7 @@ namespace planning_test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index 2d7fe52e5dcad..a6652e83733d2 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -23,7 +23,7 @@ Note that the velocity is just taken over from the input path. | ------------------ | -------------------------------------------------- | -------------------------------------------------- | | `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | | `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | -| `~/input/objects` | autoware_auto_perception_msgs/msg/PredictedObjects | objects to avoid | +| `~/input/objects` | autoware_perception_msgs/msg/PredictedObjects | objects to avoid | ### output diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp index 29cfddfbf8632..2260db16fd02f 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -20,7 +20,7 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include +#include #include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp index 8a792eba2f237..37fcebc908cec 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -15,7 +15,7 @@ #ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ #define PATH_SAMPLER__TYPE_ALIAS_HPP_ -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -38,7 +38,7 @@ using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; // perception -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; // navigation using nav_msgs::msg::Odometry; // visualization diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml index 22303a021ff63..cca24f31b95a8 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -13,6 +13,7 @@ autoware_cmake + autoware_perception_msgs autoware_auto_planning_msgs bezier_sampler frenet_planner diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp index 7a6509d5a632c..6c51339fa257c 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp @@ -61,11 +61,11 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node const std::string & lanelet2_output_file_path, const std::vector & route_lane_ids, const std::vector & optimized_traj_points); - HADMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; + LaneletMapBin::ConstSharedPtr map_bin_ptr_{nullptr}; std::shared_ptr route_handler_ptr_{nullptr}; // publisher - rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; + rclcpp::Publisher::SharedPtr pub_map_bin_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; rclcpp::Publisher::SharedPtr pub_debug_unsafe_footprints_{nullptr}; diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 2d7b7314e5e32..8f9f197f654c5 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -17,7 +17,7 @@ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" @@ -28,7 +28,7 @@ namespace static_centerline_optimizer { -using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 4d2e5a77daaa3..36578d6ee98a1 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -15,7 +15,7 @@ rosidl_default_generators - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_map_msgs behavior_path_planner diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 51f090d7cc2e3..19b245cc90273 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -81,7 +81,7 @@ As mentioned in stop condition section, it prevents chattering by changing thres | Name | Type | Description | | ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | | `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | | `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | | `/tf` | `tf2_msgs::msg::TFMessage` | TF | | `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | diff --git a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp index c40fef5b0c43e..e57867d2c5120 100644 --- a/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp +++ b/planning/surround_obstacle_checker/include/surround_obstacle_checker/node.hpp @@ -22,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -46,8 +46,8 @@ namespace surround_obstacle_checker { -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::Shape; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::Shape; using motion_utils::VehicleStopChecker; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 1f99ba8d2fcba..6f353fa82f9af 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,7 +14,7 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_auto_perception_msgs + autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 diagnostic_msgs diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 1b981fcddb155..3de5a83685864 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -50,7 +50,7 @@ namespace surround_obstacle_checker namespace bg = boost::geometry; using Point2d = bg::model::d2::point_xy; using Polygon2d = bg::model::polygon; -using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::pose2transform; diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md index 8a72c976621f8..835c5a2c15059 100644 --- a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -81,7 +81,7 @@ Stop condition の項で述べたように、状態によって障害物判定 | Name | Type | Description | | ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | | `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | -| `/perception/object_recognition/objects` | `autoware_auto_perception_msgs::msg::PredictedObjects` | Dynamic objects | +| `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | | `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | | `/tf` | `tf2_msgs::msg::TFMessage` | TF | | `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | diff --git a/simulator/dummy_perception_publisher/package.xml b/simulator/dummy_perception_publisher/package.xml index 0632bd90b2c22..4704024214d9e 100644 --- a/simulator/dummy_perception_publisher/package.xml +++ b/simulator/dummy_perception_publisher/package.xml @@ -18,7 +18,7 @@ ament_lint_auto autoware_lint_common - autoware_auto_perception_msgs + autoware_perception_msgs geometry_msgs libpcl-all-dev pcl_conversions diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index 9b3f01fa4b267..8f46ad23b5bb5 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -14,7 +14,7 @@ #include -#include +#include #include #include @@ -25,7 +25,7 @@ class EmptyObjectsPublisher : public rclcpp::Node EmptyObjectsPublisher() : Node("empty_objects_publisher") { empty_objects_pub_ = - this->create_publisher( + this->create_publisher( "~/output/objects", 1); using std::chrono_literals::operator""ms; @@ -34,13 +34,13 @@ class EmptyObjectsPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr empty_objects_pub_; rclcpp::TimerBase::SharedPtr timer_; void timerCallback() { - autoware_auto_perception_msgs::msg::PredictedObjects empty_objects; + autoware_perception_msgs::msg::PredictedObjects empty_objects; empty_objects.header.frame_id = "map"; empty_objects.header.stamp = this->now(); empty_objects_pub_->publish(empty_objects); From 3249869ee0f8ab7433c171ff2470df33c6daa2ec Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Sat, 25 Nov 2023 06:18:21 +0000 Subject: [PATCH 2/3] style(pre-commit): autofix --- .../README.md | 4 ++-- .../object_detection/object_polygon_detail.hpp | 6 +++--- .../package.xml | 2 +- .../object_detection/object_polygon_detail.cpp | 3 --- common/component_interface_specs/package.xml | 2 +- .../predicted_path_utils.hpp | 6 +++--- .../src/predicted_path_utils.cpp | 5 ++--- .../geometry/boost_polygon_utils.hpp | 2 +- common/tier4_autoware_utils/package.xml | 2 +- .../predicted_path_checker_node.hpp | 2 +- .../include/predicted_path_checker/utils.hpp | 4 ++-- evaluator/planning_evaluator/README.md | 8 ++++---- .../metrics/obstacle_metrics.hpp | 4 ++-- .../planning_evaluator/metrics_calculator.hpp | 4 ++-- .../planning_evaluator_node.hpp | 4 ++-- evaluator/planning_evaluator/package.xml | 2 +- .../test/test_planning_evaluator_node.cpp | 2 +- perception/map_based_prediction/README.md | 10 +++++----- .../map_based_prediction_node.hpp | 10 +++++----- .../map_based_prediction/path_generator.hpp | 8 ++++---- perception/radar_object_tracker/README.md | 2 +- planning/behavior_path_planner/README.md | 12 ++++++------ .../behavior_path_planner_node.hpp | 6 +++--- .../behavior_path_planner/data_manager.hpp | 6 +++--- .../marker_utils/avoidance/debug.hpp | 4 ++-- .../marker_utils/utils.hpp | 4 ++-- .../avoidance/avoidance_module.hpp | 2 +- .../dynamic_avoidance_module.hpp | 4 ++-- .../utils/avoidance/avoidance_module_data.hpp | 4 ++-- .../utils/drivable_area_expansion/types.hpp | 4 ++-- .../geometric_parallel_parking.hpp | 2 +- .../utils/goal_planner/util.hpp | 4 ++-- .../utils/lane_change/utils.hpp | 4 ++-- .../path_safety_checker/objects_filtering.hpp | 4 ++-- .../utils/start_planner/util.hpp | 4 ++-- .../behavior_path_planner/utils/utils.hpp | 8 ++++---- .../src/marker_utils/lane_change/debug.cpp | 2 +- .../src/utils/lane_change/utils.cpp | 2 +- .../package.xml | 2 +- .../src/scene.hpp | 5 ++--- .../src/scene_crosswalk.hpp | 2 +- .../src/scene_intersection.cpp | 6 ++---- .../src/scene_merge_from_private_road.hpp | 2 +- .../src/util.cpp | 9 +++------ .../src/util_type.hpp | 2 +- .../package.xml | 2 +- .../src/scene_no_stopping_area.cpp | 3 +-- .../src/scene_no_stopping_area.hpp | 5 ++--- .../package.xml | 2 +- .../src/manager.hpp | 2 +- .../src/occlusion_spot_utils.hpp | 10 +++++----- .../src/scene_occlusion_spot.hpp | 2 +- .../package.xml | 2 +- .../src/manager.hpp | 2 +- .../src/scene_out_of_lane.hpp | 2 +- .../src/types.hpp | 2 +- planning/behavior_velocity_planner/README.md | 18 +++++++++--------- planning/behavior_velocity_planner/package.xml | 2 +- .../behavior_velocity_planner/src/node.hpp | 2 +- .../src/planner_manager.hpp | 2 +- .../planner_data.hpp | 2 +- .../utilization/debug.hpp | 2 +- .../utilization/util.hpp | 4 ++-- .../behavior_velocity_run_out_module/README.md | 12 ++++++------ .../package.xml | 2 +- .../src/dynamic_obstacle.hpp | 4 ++-- .../src/scene.hpp | 2 +- .../src/utils.hpp | 6 +++--- planning/costmap_generator/README.md | 10 +++++----- .../costmap_generator/costmap_generator.hpp | 5 ++--- planning/costmap_generator/package.xml | 2 +- planning/obstacle_cruise_planner/README.md | 8 ++++---- .../obstacle_cruise_planner/type_alias.hpp | 6 +++--- planning/obstacle_cruise_planner/package.xml | 2 +- planning/obstacle_stop_planner/README.md | 14 +++++++------- .../adaptive_cruise_control.hpp | 2 +- .../include/obstacle_stop_planner/node.hpp | 4 ++-- .../obstacle_stop_planner/planner_data.hpp | 2 +- .../obstacle_stop_planner/planner_utils.hpp | 2 +- planning/obstacle_stop_planner/package.xml | 2 +- planning/obstacle_velocity_limiter/README.md | 14 +++++++------- .../obstacle_velocity_limiter.hpp | 4 ++-- .../obstacle_velocity_limiter_node.hpp | 2 +- .../obstacle_velocity_limiter/types.hpp | 4 ++-- planning/obstacle_velocity_limiter/package.xml | 2 +- .../src/obstacle_velocity_limiter.cpp | 4 ++-- .../perception_replayer_common.py | 6 ++---- .../planning_interface_test_manager.hpp | 4 ++-- .../path_sampler/README.md | 8 ++++---- .../include/path_sampler/prepare_inputs.hpp | 2 +- .../include/path_sampler/type_alias.hpp | 2 +- .../path_sampler/package.xml | 2 +- .../static_centerline_optimizer/type_alias.hpp | 4 ++-- .../static_centerline_optimizer/package.xml | 2 +- planning/surround_obstacle_checker/README.md | 12 ++++++------ planning/surround_obstacle_checker/package.xml | 2 +- .../surround_obstacle_checker-design.ja.md | 12 ++++++------ .../src/empty_objects_publisher.cpp | 8 +++----- 98 files changed, 212 insertions(+), 229 deletions(-) diff --git a/common/autoware_auto_perception_rviz_plugin/README.md b/common/autoware_auto_perception_rviz_plugin/README.md index dcc4c84a56c0a..2ea4c32eae9d6 100644 --- a/common/autoware_auto_perception_rviz_plugin/README.md +++ b/common/autoware_auto_perception_rviz_plugin/README.md @@ -45,8 +45,8 @@ Overwrite tracking results with detection results. #### Input Types -| Name | Type | Description | -| ---- | ------------------------------------------------------ | ----------------------- | +| Name | Type | Description | +| ---- | ------------------------------------------------- | ----------------------- | | | `autoware_perception_msgs::msg::PredictedObjects` | prediction result array | #### Visualization Result diff --git a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp index 2ddd2dd07b898..3160c83f5d899 100644 --- a/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp +++ b/common/autoware_auto_perception_rviz_plugin/include/object_detection/object_polygon_detail.hpp @@ -19,12 +19,12 @@ #include #include -#include -#include -#include #include #include #include +#include +#include +#include #include #include #include diff --git a/common/autoware_auto_perception_rviz_plugin/package.xml b/common/autoware_auto_perception_rviz_plugin/package.xml index e5f8bec2c8e49..b42d53b29c02a 100644 --- a/common/autoware_auto_perception_rviz_plugin/package.xml +++ b/common/autoware_auto_perception_rviz_plugin/package.xml @@ -17,8 +17,8 @@ libboost-dev qtbase5-dev - autoware_perception_msgs autoware_auto_perception_msgs + autoware_perception_msgs rviz_common rviz_default_plugins diff --git a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp index d34b3dfd80423..9f0a96e3203bf 100644 --- a/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp +++ b/common/autoware_auto_perception_rviz_plugin/src/object_detection/object_polygon_detail.cpp @@ -346,7 +346,6 @@ visualization_msgs::msg::Marker::SharedPtr get_shape_marker_ptr( return marker_ptr; } - visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( const autoware_auto_perception_msgs::msg::Shape & shape_msg, const geometry_msgs::msg::Point & centroid, const geometry_msgs::msg::Quaternion & orientation, @@ -411,7 +410,6 @@ visualization_msgs::msg::Marker::SharedPtr get_2d_shape_marker_ptr( return marker_ptr; } - void calc_bounding_box_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) @@ -730,7 +728,6 @@ void calc_2d_bounding_box_bottom_line_list( points.push_back(point); } - void calc_cylinder_line_list( const autoware_auto_perception_msgs::msg::Shape & shape, std::vector & points) diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index c8063bfb74d21..26fad66089e71 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -20,9 +20,9 @@ ament_cmake_test autoware_adapi_v1_msgs - autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs autoware_planning_msgs nav_msgs rcl diff --git a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp index 4412e10485c77..1cd838e090560 100644 --- a/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp +++ b/common/object_recognition_utils/include/object_recognition_utils/predicted_path_utils.hpp @@ -58,9 +58,9 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( * @return resampled path */ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, - const bool use_spline_for_xy = true, const bool use_spline_for_z = false); + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy = true, + const bool use_spline_for_z = false); } // namespace object_recognition_utils #endif // OBJECT_RECOGNITION_UTILS__PREDICTED_PATH_UTILS_HPP_ diff --git a/common/object_recognition_utils/src/predicted_path_utils.cpp b/common/object_recognition_utils/src/predicted_path_utils.cpp index 1f08afb86eab9..fe9499b4eec33 100644 --- a/common/object_recognition_utils/src/predicted_path_utils.cpp +++ b/common/object_recognition_utils/src/predicted_path_utils.cpp @@ -100,9 +100,8 @@ autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( } autoware_perception_msgs::msg::PredictedPath resamplePredictedPath( - const autoware_perception_msgs::msg::PredictedPath & path, - const double sampling_time_interval, const double sampling_horizon, const bool use_spline_for_xy, - const bool use_spline_for_z) + const autoware_perception_msgs::msg::PredictedPath & path, const double sampling_time_interval, + const double sampling_horizon, const bool use_spline_for_xy, const bool use_spline_for_z) { if (path.path.empty()) { throw std::invalid_argument("Predicted Path is empty"); diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 0f823a3a0a605..1c4113ac70d3b 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -18,8 +18,8 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" #include -#include #include +#include #include #include diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 57e52fb4d4a82..9c496e6806b2c 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -13,9 +13,9 @@ autoware_cmake autoware_auto_perception_msgs - autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_vehicle_msgs + autoware_perception_msgs builtin_interfaces diagnostic_msgs geometry_msgs diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index ed19871aae1cf..29136353f9a8b 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -27,8 +27,8 @@ #include #include -#include #include +#include #include #include #include diff --git a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp index 979311c219fa3..ea7e44873154c 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/utils.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/utils.hpp @@ -20,8 +20,8 @@ #include #include -#include #include +#include #include #include #include @@ -39,9 +39,9 @@ namespace utils { +using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::TransformStamped; diff --git a/evaluator/planning_evaluator/README.md b/evaluator/planning_evaluator/README.md index 82c1886d1c68b..a0cc668b3c3c8 100644 --- a/evaluator/planning_evaluator/README.md +++ b/evaluator/planning_evaluator/README.md @@ -48,10 +48,10 @@ Adding a new metric `M` requires the following steps: ### Inputs -| Name | Type | Description | -| ------------------------------ | ------------------------------------------------------ | ------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | -| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | +| Name | Type | Description | +| ------------------------------ | ------------------------------------------------- | ------------------------------------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Main trajectory to evaluate | +| `~/input/reference_trajectory` | `autoware_auto_planning_msgs::msg::Trajectory` | Reference trajectory to use for deviation metrics | | `~/input/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Obstacles | ### Outputs diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp index 23e5341f20c09..fdca39f1e6b39 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/obstacle_metrics.hpp @@ -17,15 +17,15 @@ #include "planning_evaluator/stat.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" namespace planning_diagnostics { namespace metrics { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_perception_msgs::msg::PredictedObjects; /** * @brief calculate the distance to the closest obstacle at each point of the trajectory diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 6188ab98f5543..8c0f6ded04cd7 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -18,9 +18,9 @@ #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" @@ -28,9 +28,9 @@ namespace planning_diagnostics { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index cce95519babf2..b1feefc8a85f5 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -21,9 +21,9 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "nav_msgs/msg/odometry.hpp" @@ -36,9 +36,9 @@ namespace planning_diagnostics { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index 003373f5c419f..5574a246963e4 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -13,8 +13,8 @@ ament_cmake_auto autoware_cmake - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs autoware_planning_msgs diagnostic_msgs eigen diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index eb63e641cb323..8407b89f41d4b 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -20,8 +20,8 @@ #include -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 78c043b70bad3..251305181dbb8 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -158,11 +158,11 @@ If the target object is inside the road or crosswalk, this module outputs one or ### Output -| Name | Type | Description | -| ------------------------ | ------------------------------------------------------ | ------------------------------------------------------------------------------------- | -| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | -| `~/output/objects` | `autoware_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | -| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | +| Name | Type | Description | +| ------------------------ | ---------------------------------------------------- | ------------------------------------------------------------------------------------- | +| `~/input/objects` | `autoware_auto_perception_msgs::msg::TrackedObjects` | tracking objects. Default is set to `/perception/object_recognition/tracking/objects` | +| `~/output/objects` | `autoware_perception_msgs::msg::PredictedObjects` | tracking objects with predicted path. | +| `~/objects_path_markers` | `visualization_msgs::msg::MarkerArray` | marker for visualization. | ## Parameters diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 9b51d5fbbf0ee..8a70e10ed7e8c 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -21,9 +21,9 @@ #include #include -#include #include #include +#include #include #include #include @@ -90,15 +90,15 @@ struct PredictedRefPath using LaneletsData = std::vector; using ManeuverProbability = std::unordered_map; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; +using autoware_map_msgs::msg::LaneletMapBin; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjectKinematics; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; -using autoware_auto_perception_msgs::msg::TrackedObjects; -using autoware_map_msgs::msg::LaneletMapBin; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::StringStamped; diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 60da84a848b52..5a146df65e36a 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include #include #include @@ -30,14 +30,14 @@ namespace map_based_prediction { +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjectKinematics; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::TrackedObject; -using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; -using autoware_auto_perception_msgs::msg::TrackedObjects; struct FrenetPoint { diff --git a/perception/radar_object_tracker/README.md b/perception/radar_object_tracker/README.md index b9c758e0872e1..4cbcf12ce18c1 100644 --- a/perception/radar_object_tracker/README.md +++ b/perception/radar_object_tracker/README.md @@ -29,7 +29,7 @@ See more details in the [models.md](models.md). | Name | Type | Description | | ------------- | ----------------------------------------------------- | ---------------- | | `~/input` | `autoware_auto_perception_msgs::msg::DetectedObjects` | Detected objects | -| `/vector/map` | `autoware_map_msgs::msg::LaneletMapBin` | Map data | +| `/vector/map` | `autoware_map_msgs::msg::LaneletMapBin` | Map data | ### Output diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index a4bcd2d90c750..d7ee97a0c0309 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -91,13 +91,13 @@ The current behavior tree structure is shown below. Each modules (LaneChange, Av ### input -| Name | Type | Description | -| :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ | -| ~/input/route | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/vector_map | `autoware_map_msgs::msg::LaneletMapBin` | map information. | +| Name | Type | Description | +| :----------------------------- | :------------------------------------------------ | :------------------------------------------------------------------------------------ | +| ~/input/route | `autoware_planning_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/vector_map | `autoware_map_msgs::msg::LaneletMapBin` | map information. | | ~/input/objects | `autoware_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | ## General features of behavior path planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index dca7f0de73461..7b816f185e104 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -23,12 +23,12 @@ #include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include -#include #include #include #include #include #include +#include #include #include #include @@ -51,13 +51,13 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_perception_msgs::msg::PredictedObject; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using autoware_planning_msgs::msg::PoseWithUuidStamped; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 39be668f7df7a..47a496f17d491 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -25,10 +25,10 @@ #include #include -#include #include #include #include +#include #include #include #include @@ -49,11 +49,11 @@ namespace behavior_path_planner { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_perception_msgs::msg::PredictedObject; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficSignal; using autoware_planning_msgs::msg::PoseWithUuidStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 985332abd6a90..7d8b432455f7e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -20,8 +20,8 @@ #include -#include #include +#include #include #include @@ -32,8 +32,8 @@ namespace marker_utils::avoidance_marker { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using behavior_path_planner::AvoidancePlanningData; using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp index 21e2f41a78266..81119d00fe7a2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp @@ -18,9 +18,9 @@ #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include #include #include -#include #include #include @@ -32,9 +32,9 @@ namespace marker_utils { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::DrivableLanes; using behavior_path_planner::ShiftLineArray; using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index b9a57a712a041..428ad392792ac 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -23,9 +23,9 @@ #include -#include #include #include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 11f573c13dfce..0f6a050524f75 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -20,10 +20,10 @@ #include #include -#include -#include #include #include +#include +#include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 29fa7ecc0c69c..f13cb35ae2689 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -22,8 +22,8 @@ #include #include -#include #include +#include #include #include @@ -38,9 +38,9 @@ namespace behavior_path_planner { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp index 4625f127b145d..4f027e949a1fe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp @@ -17,19 +17,19 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include #include #include +#include #include #include namespace drivable_area_expansion { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index d8092cfab4ba4..34f9f61bf60d2 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 0f850de3249e8..0e102caa67905 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -20,9 +20,9 @@ #include +#include #include #include -#include #include #include @@ -36,9 +36,9 @@ namespace behavior_path_planner { namespace goal_planner_utils { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using visualization_msgs::msg::Marker; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 17759a037eec2..c445a9b87b451 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -23,8 +23,8 @@ #include -#include #include +#include #include #include @@ -35,10 +35,10 @@ namespace behavior_path_planner::utils::lane_change { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp index f262015fa9bf5..2e66b656ae655 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp @@ -18,9 +18,9 @@ #include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include #include #include -#include #include #include @@ -33,9 +33,9 @@ namespace behavior_path_planner::utils::path_safety_checker { +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; /** * @brief Filters objects based on object centroid position. diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index d2b9b555cf5f3..c3ca876ded771 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -21,9 +21,9 @@ #include +#include #include #include -#include #include #include @@ -34,9 +34,9 @@ namespace behavior_path_planner::start_planner_utils { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 22b7bf7c7d59c..3a84b4db4cd3e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -22,13 +22,13 @@ #include #include +#include +#include +#include #include #include #include #include -#include -#include -#include #include #include #include @@ -54,10 +54,10 @@ using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; -using autoware_perception_msgs::msg::Shape; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::Shape; using drivable_area_expansion::DrivableAreaExpansionParameters; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a7e50cc0cd933..55b8d4d9dc500 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -19,8 +19,8 @@ #include #include -#include #include +#include #include #include diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 2eb0b31497460..6f294a97337d4 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -45,9 +45,9 @@ namespace behavior_path_planner::utils::lane_change { +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using route_handler::RouteHandler; using tier4_autoware_utils::LineString2d; diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index d862bea16d106..108fef246d2f1 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 21ee6332a9d60..03a16dc04f7b3 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -20,9 +20,9 @@ #include #include +#include #include #include -#include #include #include @@ -216,8 +216,7 @@ class BlindSpotModule : public SceneModuleInterface * @param time_thr time threshold to cut path */ void cutPredictPathWithDuration( - autoware_perception_msgs::msg::PredictedObjects * objects_ptr, - const double time_thr) const; + autoware_perception_msgs::msg::PredictedObjects * objects_ptr, const double time_thr) const; StateMachine state_machine_; //! for state diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index a2a05bad00efa..4755263d6b98c 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -49,10 +49,10 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::TrafficSignalElement; using lanelet::autoware::Crosswalk; using tier4_api_msgs::msg::CrosswalkStatus; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 50e4fdd1f1ce1..078c9a12e961b 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -75,10 +75,8 @@ static bool isTargetCollisionVehicleType( const autoware_perception_msgs::msg::PredictedObject & object) { if ( - object.classification.at(0).label == - autoware_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.at(0).label == diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index c1ffd910be863..d129dc4611eda 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index ee2fcadc4209d..c2fda54ced6e5 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -1132,14 +1132,11 @@ geometry_msgs::msg::Pose getObjectPoseWithVelocityDirection( return obj_pose; } -static bool isTargetStuckVehicleType( - const autoware_perception_msgs::msg::PredictedObject & object) +static bool isTargetStuckVehicleType(const autoware_perception_msgs::msg::PredictedObject & object) { if ( - object.classification.at(0).label == - autoware_perception_msgs::msg::ObjectClassification::CAR || - object.classification.at(0).label == - autoware_perception_msgs::msg::ObjectClassification::BUS || + object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::CAR || + object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::BUS || object.classification.at(0).label == autoware_perception_msgs::msg::ObjectClassification::TRUCK || object.classification.at(0).label == diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index 130ae23c0ddc4..075ad08137339 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index 6c710b581c8e0..42916f82dbd4e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index dc5000707bdaa..ae9ed730f92a7 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -220,8 +220,7 @@ bool NoStoppingAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason bool NoStoppingAreaModule::checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr) + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr) { // stuck points by predicted objects for (const auto & object : predicted_obj_arr_ptr->objects) { diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 008c888067564..2fdce15877e77 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -24,10 +24,10 @@ #include #include +#include #include #include #include -#include #include @@ -111,8 +111,7 @@ class NoStoppingAreaModule : public SceneModuleInterface */ bool checkStuckVehiclesInNoStoppingArea( const Polygon2d & poly, - const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & - predicted_obj_arr_ptr); + const autoware_perception_msgs::msg::PredictedObjects::ConstSharedPtr & predicted_obj_arr_ptr); /** * @brief Check if there is a stop line in "stop line detect area". diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index f722177d607b1..158381865dd8a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,8 +16,8 @@ ament_cmake_auto autoware_cmake - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs grid_map_ros diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index b64a5023e86d7..ee9254cc8668c 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -23,9 +23,9 @@ #include #include +#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 0c131225dbd27..920095aea65e4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -23,11 +23,11 @@ #include #include +#include +#include #include #include #include -#include -#include #include #include @@ -50,13 +50,13 @@ namespace behavior_velocity_planner { -using autoware_perception_msgs::msg::ObjectClassification; -using autoware_perception_msgs::msg::PredictedObject; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using lanelet::ArcCoordinates; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index 7d3ec5b908300..3aeb290343940 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index 089672b517d1d..ceb880d7808d1 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,8 +14,8 @@ ament_cmake_auto autoware_cmake - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 0f32578ae74a5..794233448d988 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -22,9 +22,9 @@ #include #include +#include #include #include -#include #include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index bcd22d03d6e03..7002c88bc2843 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index 8cf22e78cece5..7869eff0b5e29 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 3017e65e4b2ec..e7eeeb198c07f 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -29,15 +29,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------------------- | ---------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | -| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_perception_msgs::msg::TrafficSignalArray | traffic light states | ## Output topics diff --git a/planning/behavior_velocity_planner/package.xml b/planning/behavior_velocity_planner/package.xml index 1df100c666590..bd15bed0f7b0b 100644 --- a/planning/behavior_velocity_planner/package.xml +++ b/planning/behavior_velocity_planner/package.xml @@ -34,9 +34,9 @@ rosidl_default_generators - autoware_perception_msgs autoware_auto_planning_msgs autoware_map_msgs + autoware_perception_msgs behavior_velocity_planner_common diagnostic_msgs eigen diff --git a/planning/behavior_velocity_planner/src/node.hpp b/planning/behavior_velocity_planner/src/node.hpp index cc7786523ce29..451e64b27b382 100644 --- a/planning/behavior_velocity_planner/src/node.hpp +++ b/planning/behavior_velocity_planner/src/node.hpp @@ -23,10 +23,10 @@ #include #include -#include #include #include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner/src/planner_manager.hpp b/planning/behavior_velocity_planner/src/planner_manager.hpp index 6d45b009e30ad..e76d7b58deb32 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/behavior_velocity_planner/src/planner_manager.hpp @@ -20,10 +20,10 @@ #include #include -#include #include #include #include +#include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 25336603ab676..f7dc4c4f9da5a 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp index 15db7457382d5..c8761e77cf6b5 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index bc308434f74cf..8668cdf9bebcd 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include #include @@ -63,9 +63,9 @@ using LineString2d = tier4_autoware_utils::LineString2d; using Polygon2d = tier4_autoware_utils::Polygon2d; using BasicPolygons2d = std::vector; using Polygons2d = std::vector; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using tier4_planning_msgs::msg::StopFactor; using tier4_planning_msgs::msg::StopReason; diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/behavior_velocity_run_out_module/README.md index 345fbb55681a0..abd9b2f7db82b 100644 --- a/planning/behavior_velocity_run_out_module/README.md +++ b/planning/behavior_velocity_run_out_module/README.md @@ -71,14 +71,14 @@ This module can handle multiple types of obstacles by creating abstracted dynami Abstracted obstacle data has following information. -| Name | Type | Description | -| ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | -| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | +| Name | Type | Description | +| ---------------- | ------------------------------------------------------------------ | ---------------------------------------------------------------------------------------------------------------------- | +| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | | classifications | `std::vector` | classifications with probability | | shape | `autoware_perception_msgs::msg::Shape` | shape of the obstacle | -| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | -| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | -| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | +| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | +| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | +| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](.#Collision-detection). diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/behavior_velocity_run_out_module/package.xml index 8fac6be06686f..072e4b6639fa1 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/behavior_velocity_run_out_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs behavior_velocity_planner_common eigen geometry_msgs diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index 3c4731aba8560..ca2517386febf 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -43,11 +43,11 @@ namespace behavior_velocity_planner { +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPointWithLaneId; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/behavior_velocity_run_out_module/src/scene.hpp index fd42805e2f40c..aae80c3b0291b 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/behavior_velocity_run_out_module/src/scene.hpp @@ -29,9 +29,9 @@ namespace behavior_velocity_planner { namespace bg = boost::geometry; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; using run_out_utils::PoseWithRange; using tier4_debug_msgs::msg::Float32Stamped; diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/behavior_velocity_run_out_module/src/utils.hpp index ff2c6cabe5e3d..afa6f981d2bd5 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/behavior_velocity_run_out_module/src/utils.hpp @@ -19,8 +19,8 @@ #include #include -#include #include +#include #include #include @@ -31,11 +31,11 @@ namespace behavior_velocity_planner namespace run_out_utils { namespace bg = boost::geometry; +using autoware_auto_planning_msgs::msg::PathPoint; +using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::PathPoint; -using autoware_auto_planning_msgs::msg::PathWithLaneId; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index 74dd462be275a..5701dcc263911 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -6,12 +6,12 @@ This node reads `PointCloud` and/or `DynamicObjectArray` and creates an `Occupan ### Input topics -| Name | Type | Description | -| ------------------------- | ----------------------------------------------- | ---------------------------------------------------------------------------- | +| Name | Type | Description | +| ------------------------- | ------------------------------------------ | ---------------------------------------------------------------------------- | | `~input/objects` | autoware_perception_msgs::PredictedObjects | predicted objects, for obstacles areas | -| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | -| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | -| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | +| `~input/points_no_ground` | sensor_msgs::PointCloud2 | ground-removed points, for obstacle areas which can't be detected as objects | +| `~input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map, for drivable areas | +| `~input/scenario` | tier4_planning_msgs::Scenario | scenarios to be activated, for node activation | ### Output topics diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index 0b21a2e5b74f9..31c8a05cc3c60 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -53,8 +53,8 @@ #include #include -#include #include +#include #include #include @@ -114,8 +114,7 @@ class CostmapGenerator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_occupancy_grid_; rclcpp::Subscription::SharedPtr sub_points_; - rclcpp::Subscription::SharedPtr - sub_objects_; + rclcpp::Subscription::SharedPtr sub_objects_; rclcpp::Subscription::SharedPtr sub_lanelet_bin_map_; rclcpp::Subscription::SharedPtr sub_scenario_; diff --git a/planning/costmap_generator/package.xml b/planning/costmap_generator/package.xml index aa01572c9a352..dc94d74dce8ae 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/costmap_generator/package.xml @@ -15,8 +15,8 @@ ament_cmake_auto autoware_cmake - autoware_perception_msgs autoware_map_msgs + autoware_perception_msgs grid_map_ros lanelet2_extension libpcl-all-dev diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index ec07fab7b0511..15072f1512eec 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -15,11 +15,11 @@ The `obstacle_cruise_planner` package has following modules. ### Input topics -| Name | Type | Description | -| -------------------- | ----------------------------------------------- | ---------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | +| Name | Type | Description | +| -------------------- | ------------------------------------------ | ---------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | | `~/input/objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp index 2a10ddbcd9da1..0f72f4d332699 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -18,9 +18,9 @@ #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_perception_msgs/msg/predicted_object.hpp" #include "autoware_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" @@ -36,13 +36,13 @@ using autoware_adapi_v1_msgs::msg::VelocityFactor; using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; using autoware_perception_msgs::msg::Shape; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; using geometry_msgs::msg::AccelStamped; using geometry_msgs::msg::AccelWithCovarianceStamped; using geometry_msgs::msg::Twist; diff --git a/planning/obstacle_cruise_planner/package.xml b/planning/obstacle_cruise_planner/package.xml index 5a839db4771b5..f3291d1870024 100644 --- a/planning/obstacle_cruise_planner/package.xml +++ b/planning/obstacle_cruise_planner/package.xml @@ -17,8 +17,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs geometry_msgs interpolation lanelet2_extension diff --git a/planning/obstacle_stop_planner/README.md b/planning/obstacle_stop_planner/README.md index c3c101a8e4994..e5235e074b7fa 100644 --- a/planning/obstacle_stop_planner/README.md +++ b/planning/obstacle_stop_planner/README.md @@ -13,14 +13,14 @@ ### Input topics -| Name | Type | Description | -| --------------------------- | ----------------------------------------------- | ------------------- | -| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | -| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | -| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | +| Name | Type | Description | +| --------------------------- | ------------------------------------------ | ------------------- | +| `~/input/pointcloud` | sensor_msgs::PointCloud2 | obstacle pointcloud | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory | +| `~/input/vector_map` | autoware_map_msgs::msg::LaneletMapBin | vector map | +| `~/input/odometry` | nav_msgs::Odometry | vehicle velocity | | `~/input/dynamic_objects` | autoware_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | +| `~/input/expand_stop_range` | tier4_planning_msgs::msg::ExpandStopRange | expand stop range | ### Output topics diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp index d7bef2ea4a05b..4beb46b42fc6c 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/adaptive_cruise_control.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index 821f912c0d715..9e783cbdfa6c9 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -28,8 +28,8 @@ #include #include -#include #include +#include #include #include #include @@ -75,9 +75,9 @@ using nav_msgs::msg::Odometry; using sensor_msgs::msg::PointCloud2; using std_msgs::msg::Header; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; using tier4_autoware_utils::StopWatch; diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp index c986b25d62e35..dcdf25188853b 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_data.hpp @@ -17,8 +17,8 @@ #include -#include #include +#include #include #include diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp index 01eddc35d5dd2..4a64e79ef4ad2 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/planner_utils.hpp @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include #include #include diff --git a/planning/obstacle_stop_planner/package.xml b/planning/obstacle_stop_planner/package.xml index 4ff8c4019844f..70743397127a2 100644 --- a/planning/obstacle_stop_planner/package.xml +++ b/planning/obstacle_stop_planner/package.xml @@ -20,8 +20,8 @@ autoware_cmake autoware_adapi_v1_msgs - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs diagnostic_msgs motion_utils nav_msgs diff --git a/planning/obstacle_velocity_limiter/README.md b/planning/obstacle_velocity_limiter/README.md index 7d30532e3a8be..c45de1f4cd366 100644 --- a/planning/obstacle_velocity_limiter/README.md +++ b/planning/obstacle_velocity_limiter/README.md @@ -146,14 +146,14 @@ For example a value of `1` means all trajectory points will be evaluated while a ### Inputs -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------ | -------------------------------------------------- | -| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | -| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | -| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | +| Name | Type | Description | +| ----------------------------- | ------------------------------------------- | -------------------------------------------------- | +| `~/input/trajectory` | `autoware_auto_planning_msgs/Trajectory` | Reference trajectory | +| `~/input/occupancy_grid` | `nav_msgs/OccupancyGrid` | Occupancy grid with obstacle information | +| `~/input/obstacle_pointcloud` | `sensor_msgs/PointCloud2` | Pointcloud containing only obstacle points | | `~/input/dynamic_obstacles` | `autoware_perception_msgs/PredictedObjects` | Dynamic objects | -| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | -| `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | +| `~/input/odometry` | `nav_msgs/Odometry` | Odometry used to retrieve the current ego velocity | +| `~/input/map` | `autoware_map_msgs/LaneletMapBin` | Vector map used to retrieve static obstacles | ### Outputs diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp index 50d0051a6eb20..0beced62bd7d6 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter.hpp @@ -63,8 +63,8 @@ void calculateSteeringAngles(Trajectory & trajectory, const Float wheel_base); /// @param[in] min_vel minimum velocity for an object to be masked /// @return polygon masks around dynamic objects multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel); + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel); /// @brief create footprint polygons from projection lines /// @details A footprint is create for each group of lines. Each group of lines is assumed to share diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp index 6c882dc074b55..d27fc3bf55acd 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/obstacle_velocity_limiter_node.hpp @@ -24,9 +24,9 @@ #include #include -#include #include #include +#include #include #include #include diff --git a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp index 8aeec6262a8a3..87ac1a32e01e8 100644 --- a/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp +++ b/planning/obstacle_velocity_limiter/include/obstacle_velocity_limiter/types.hpp @@ -17,17 +17,17 @@ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" -#include #include #include +#include #include #include namespace obstacle_velocity_limiter { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_perception_msgs::msg::PredictedObjects; using nav_msgs::msg::OccupancyGrid; using PointCloud = sensor_msgs::msg::PointCloud2; using Float = decltype(TrajectoryPoint::longitudinal_velocity_mps); diff --git a/planning/obstacle_velocity_limiter/package.xml b/planning/obstacle_velocity_limiter/package.xml index 3171be9ffa3ff..0d80474e223f5 100644 --- a/planning/obstacle_velocity_limiter/package.xml +++ b/planning/obstacle_velocity_limiter/package.xml @@ -11,8 +11,8 @@ autoware_cmake eigen3_cmake_module - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs eigen grid_map_msgs grid_map_ros diff --git a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp index 46d024990a402..7c050b078c3aa 100644 --- a/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp +++ b/planning/obstacle_velocity_limiter/src/obstacle_velocity_limiter.cpp @@ -35,8 +35,8 @@ Float calculateSafeVelocity( } multi_polygon_t createPolygonMasks( - const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, - const Float buffer, const Float min_vel) + const autoware_perception_msgs::msg::PredictedObjects & dynamic_obstacles, const Float buffer, + const Float min_vel) { return createObjectPolygons(dynamic_obstacles, buffer, min_vel); } diff --git a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py index 77140458dea9d..422c8ea8b796f 100644 --- a/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py +++ b/planning/planning_debug_tools/scripts/perception_replayer/perception_replayer_common.py @@ -22,8 +22,8 @@ from autoware_perception_msgs.msg import DetectedObjects from autoware_perception_msgs.msg import PredictedObjects from autoware_perception_msgs.msg import TrackedObjects -from autoware_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray from autoware_perception_msgs.msg import TrafficSignalArray +from autoware_perception_msgs.msg import TrafficSignalArray as AutoTrafficSignalArray from geometry_msgs.msg import PoseWithCovarianceStamped from nav_msgs.msg import Odometry import psutil @@ -131,9 +131,7 @@ def load_rosbag(self, rosbag2_path: str): self.rosbag_ego_odom_data.append((stamp, msg)) if topic == traffic_signals_topic: self.rosbag_traffic_signals_data.append((stamp, msg)) - self.is_auto_traffic_signals = ( - "autoware_perception_msgs" in type(msg).__module__ - ) + self.is_auto_traffic_signals = "autoware_perception_msgs" in type(msg).__module__ def kill_online_perception_node(self): # kill node if required diff --git a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp index 8e8ca89637b09..5282a98f4c60d 100644 --- a/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp +++ b/planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager.hpp @@ -36,12 +36,12 @@ #include #include -#include #include #include #include #include #include +#include #include #include #include @@ -75,11 +75,11 @@ namespace planning_test_utils { using autoware_adapi_v1_msgs::msg::OperationModeState; -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::TrafficSignalArray; using autoware_planning_msgs::msg::LaneletRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; diff --git a/planning/sampling_based_planner/path_sampler/README.md b/planning/sampling_based_planner/path_sampler/README.md index a6652e83733d2..7f5df41848f2d 100644 --- a/planning/sampling_based_planner/path_sampler/README.md +++ b/planning/sampling_based_planner/path_sampler/README.md @@ -19,10 +19,10 @@ Note that the velocity is just taken over from the input path. ### input -| Name | Type | Description | -| ------------------ | -------------------------------------------------- | -------------------------------------------------- | -| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | -| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | +| Name | Type | Description | +| ------------------ | --------------------------------------------- | -------------------------------------------------- | +| `~/input/path` | autoware_auto_planning_msgs/msg/Path | Reference path and the corresponding drivable area | +| `~/input/odometry` | nav_msgs/msg/Odometry | Current state of the ego vehicle | | `~/input/objects` | autoware_perception_msgs/msg/PredictedObjects | objects to avoid | ### output diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp index 2260db16fd02f..e5dc9ec825c52 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/prepare_inputs.hpp @@ -20,11 +20,11 @@ #include "path_sampler/type_alias.hpp" #include "sampler_common/transform/spline_transform.hpp" -#include #include #include #include #include +#include #include #include #include diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp index 37fcebc908cec..3f3aef5559d16 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/type_alias.hpp @@ -15,11 +15,11 @@ #ifndef PATH_SAMPLER__TYPE_ALIAS_HPP_ #define PATH_SAMPLER__TYPE_ALIAS_HPP_ -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/pose.hpp" #include "geometry_msgs/msg/twist.hpp" diff --git a/planning/sampling_based_planner/path_sampler/package.xml b/planning/sampling_based_planner/path_sampler/package.xml index cca24f31b95a8..06a76a547d607 100644 --- a/planning/sampling_based_planner/path_sampler/package.xml +++ b/planning/sampling_based_planner/path_sampler/package.xml @@ -13,8 +13,8 @@ autoware_cmake - autoware_perception_msgs autoware_auto_planning_msgs + autoware_perception_msgs bezier_sampler frenet_planner geometry_msgs diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp index 8f9f197f654c5..7fb9c5c5a44db 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp +++ b/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp @@ -17,24 +17,24 @@ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/path.hpp" #include "autoware_auto_planning_msgs/msg/path_point.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include "autoware_map_msgs/msg/lanelet_map_bin.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" namespace static_centerline_optimizer { -using autoware_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Path; using autoware_auto_planning_msgs::msg::PathPoint; using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_map_msgs::msg::LaneletMapBin; +using autoware_perception_msgs::msg::PredictedObjects; using autoware_planning_msgs::msg::LaneletRoute; using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_optimizer/package.xml index 36578d6ee98a1..df2eaf647b139 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_optimizer/package.xml @@ -15,9 +15,9 @@ rosidl_default_generators - autoware_perception_msgs autoware_auto_planning_msgs autoware_map_msgs + autoware_perception_msgs behavior_path_planner geometry_msgs global_parameter_loader diff --git a/planning/surround_obstacle_checker/README.md b/planning/surround_obstacle_checker/README.md index 19b245cc90273..a72f4e88d6dff 100644 --- a/planning/surround_obstacle_checker/README.md +++ b/planning/surround_obstacle_checker/README.md @@ -78,13 +78,13 @@ As mentioned in stop condition section, it prevents chattering by changing thres ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | | `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 6f353fa82f9af..7c45c8ba56db5 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -14,9 +14,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs - autoware_perception_msgs autoware_auto_planning_msgs autoware_auto_tf2 + autoware_perception_msgs diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md index 835c5a2c15059..f8a5f29e064f7 100644 --- a/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md +++ b/planning/surround_obstacle_checker/surround_obstacle_checker-design.ja.md @@ -78,13 +78,13 @@ Stop condition の項で述べたように、状態によって障害物判定 ### Input -| Name | Type | Description | -| ---------------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------ | -| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | +| Name | Type | Description | +| ---------------------------------------------- | ------------------------------------------------- | ------------------------------------------------------------------ | +| `/perception/obstacle_segmentation/pointcloud` | `sensor_msgs::msg::PointCloud2` | Pointcloud of obstacles which the ego-vehicle should stop or avoid | | `/perception/object_recognition/objects` | `autoware_perception_msgs::msg::PredictedObjects` | Dynamic objects | -| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | -| `/tf` | `tf2_msgs::msg::TFMessage` | TF | -| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | +| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Current twist | +| `/tf` | `tf2_msgs::msg::TFMessage` | TF | +| `/tf_static` | `tf2_msgs::msg::TFMessage` | TF static | ### Output diff --git a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp index 8f46ad23b5bb5..2d1ea626fb1ac 100644 --- a/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp +++ b/simulator/dummy_perception_publisher/src/empty_objects_publisher.cpp @@ -24,9 +24,8 @@ class EmptyObjectsPublisher : public rclcpp::Node public: EmptyObjectsPublisher() : Node("empty_objects_publisher") { - empty_objects_pub_ = - this->create_publisher( - "~/output/objects", 1); + empty_objects_pub_ = this->create_publisher( + "~/output/objects", 1); using std::chrono_literals::operator""ms; timer_ = rclcpp::create_timer( @@ -34,8 +33,7 @@ class EmptyObjectsPublisher : public rclcpp::Node } private: - rclcpp::Publisher::SharedPtr - empty_objects_pub_; + rclcpp::Publisher::SharedPtr empty_objects_pub_; rclcpp::TimerBase::SharedPtr timer_; void timerCallback() From c9781ebc32c698201c40d54194db9c301b01edb3 Mon Sep 17 00:00:00 2001 From: beginningfan Date: Mon, 27 Nov 2023 18:10:12 +0800 Subject: [PATCH 3/3] fix(autoware_auto_tf2): fix accidentally blocked content in cmakelists Signed-off-by: beginningfan --- common/autoware_auto_tf2/CMakeLists.txt | 28 ++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt index 81af3ee184c6a..a8ae9ec2d962e 100755 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ b/common/autoware_auto_tf2/CMakeLists.txt @@ -4,19 +4,19 @@ project(autoware_auto_tf2) find_package(autoware_cmake REQUIRED) autoware_package() -# if(BUILD_TESTING) -# ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) -# target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") -# ament_target_dependencies(test_tf2_autoware_auto_msgs -# "autoware_auto_common" -# "autoware_auto_perception_msgs" -# "autoware_auto_system_msgs" -# "autoware_auto_geometry_msgs" -# "geometry_msgs" -# "tf2" -# "tf2_geometry_msgs" -# "tf2_ros" -# ) -# endif() +if(BUILD_TESTING) + ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) + target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") + ament_target_dependencies(test_tf2_autoware_auto_msgs + "autoware_auto_common" + "autoware_auto_perception_msgs" + "autoware_auto_system_msgs" + "autoware_auto_geometry_msgs" + "geometry_msgs" + "tf2" + "tf2_geometry_msgs" + "tf2_ros" + ) +endif() ament_auto_package()