From 4658b8b9cdf0b6063c44853264ad430f9e6cf1b8 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 15:11:11 +0200 Subject: [PATCH 01/33] build(static_centerline_generator): prefix package and namespace with autoware_ Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/CMakeLists.txt | 4 ++-- .../static_centerline_generator_node.hpp | 10 +++++----- .../include/static_centerline_generator/type_alias.hpp | 4 ++-- .../include/static_centerline_generator/utils.hpp | 4 ++-- planning/static_centerline_generator/package.xml | 2 +- planning/static_centerline_generator/src/main.cpp | 2 +- .../src/static_centerline_generator_node.cpp | 2 +- 7 files changed, 14 insertions(+), 14 deletions(-) diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index 991d12097cc08..d8612eb9e3638 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(static_centerline_generator) +project(autoware_static_centerline_generator) find_package(autoware_cmake REQUIRED) @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - static_centerline_generator + autoware_static_centerline_generator "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index c1d92c06a8e05..da5d64ad6b2db 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -34,11 +34,11 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { -using static_centerline_generator::srv::LoadMap; -using static_centerline_generator::srv::PlanPath; -using static_centerline_generator::srv::PlanRoute; +using autoware::static_centerline_generator::srv::LoadMap; +using autoware::static_centerline_generator::srv::PlanPath; +using autoware::static_centerline_generator::srv::PlanRoute; struct CenterlineWithRoute { @@ -115,5 +115,5 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // vehicle info vehicle_info_util::VehicleInfo vehicle_info_; }; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 2dcb9cbbd099f..8fddef2c28842 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -26,7 +26,7 @@ #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace static_centerline_generator +namespace autoware::static_centerline_generator { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -41,6 +41,6 @@ using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index c8cf8f8b90590..9b755f80a6888 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -24,7 +24,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace utils { @@ -53,6 +53,6 @@ MarkerArray create_distance_text_marker( const geometry_msgs::msg::Pose & pose, const double dist, const std::array & marker_color, const rclcpp::Time & now, const size_t idx); } // namespace utils -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_generator/package.xml b/planning/static_centerline_generator/package.xml index 6573f6e439c43..484c386ead7f7 100644 --- a/planning/static_centerline_generator/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,7 +1,7 @@ - static_centerline_generator + autoware_static_centerline_generator 0.1.0 The static_centerline_generator package Takayuki Murooka diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 981cf54fc9274..c849fa44f188d 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; auto node = - std::make_shared(node_options); + std::make_shared(node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index ec24f47dacc27..e3b33a1adb875 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -542,7 +542,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( if (!current_lanelet_points.empty()) { // register points with lane_id - static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; + autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; points_with_lane_id.lane_id = lanelet.id(); points_with_lane_id.points = current_lanelet_points; response->points_with_lane_ids.push_back(points_with_lane_id); From d3272210857bf65b5fb10424e3c1c8b06aa73b7b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 15:11:53 +0200 Subject: [PATCH 02/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/src/main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index c849fa44f188d..7a5e215e2f975 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -21,7 +21,8 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; auto node = - std::make_shared(node_options); + std::make_shared( + node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); From 8b0b8cc0579ccfb5f300e08c6eb2934f4d50bbe6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 27 Mar 2024 15:31:14 +0100 Subject: [PATCH 03/33] build: fix CMake target Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index d8612eb9e3638..3993674c89e8c 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -29,10 +29,10 @@ ament_auto_add_executable(main if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(main - static_centerline_generator "rosidl_typesupport_cpp") + autoware_static_centerline_generator "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp") + cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") target_link_libraries(main "${cpp_typesupport_target}") endif() From 0f4cfa1bc1ebaf6c98507891d466307dd5a50484 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 2 Apr 2024 16:58:19 +0200 Subject: [PATCH 04/33] build(autoware_static_centerline_generator): more renames Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/README.md | 4 ++-- .../static_centerline_generator_node.hpp | 10 +++++----- .../include/static_centerline_generator/utils.hpp | 2 +- .../launch/run_planning_server.launch.xml | 4 ++-- .../launch/static_centerline_generator.launch.xml | 6 +++--- planning/static_centerline_generator/package.xml | 2 +- planning/static_centerline_generator/scripts/app.py | 6 +++--- planning/static_centerline_generator/src/main.cpp | 2 +- .../src/static_centerline_generator_node.cpp | 12 +++++++----- planning/static_centerline_generator/src/utils.cpp | 6 +++--- .../static_centerline_generator/srv/PlanPath.srv | 2 +- .../test/test_static_centerline_generator.test.py | 12 ++++++------ 12 files changed, 35 insertions(+), 33 deletions(-) diff --git a/planning/static_centerline_generator/README.md b/planning/static_centerline_generator/README.md index 00572b754645c..844d0af15f2a5 100644 --- a/planning/static_centerline_generator/README.md +++ b/planning/static_centerline_generator/README.md @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:= +ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= ``` FYI, port ID of the http server is 4010 by default. @@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des - `` ```sh -ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= ``` The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index da5d64ad6b2db..34ed92de182db 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -16,11 +16,11 @@ #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" -#include "static_centerline_generator/srv/load_map.hpp" -#include "static_centerline_generator/srv/plan_path.hpp" -#include "static_centerline_generator/srv/plan_route.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "autoware_static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "autoware_static_centerline_generator/srv/load_map.hpp" +#include "autoware_static_centerline_generator/srv/plan_path.hpp" +#include "autoware_static_centerline_generator/srv/plan_route.hpp" +#include "autoware_static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index 9b755f80a6888..a866d2a20b183 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -16,7 +16,7 @@ #define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ #include "route_handler/route_handler.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "autoware_static_centerline_generator/type_alias.hpp" #include diff --git a/planning/static_centerline_generator/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml index 1493078317458..6ce0054343acc 100644 --- a/planning/static_centerline_generator/launch/run_planning_server.launch.xml +++ b/planning/static_centerline_generator/launch/run_planning_server.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index ae71b0ae6e925..8c05f9a960d0f 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + @@ -79,11 +79,11 @@ - + - + diff --git a/planning/static_centerline_generator/package.xml b/planning/static_centerline_generator/package.xml index 484c386ead7f7..96e17595d49ee 100644 --- a/planning/static_centerline_generator/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -3,7 +3,7 @@ autoware_static_centerline_generator 0.1.0 - The static_centerline_generator package + The autoware_static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index c1cb0473da040..7c40e1748d85a 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -23,9 +23,9 @@ from flask_cors import CORS import rclpy from rclpy.node import Node -from static_centerline_generator.srv import LoadMap -from static_centerline_generator.srv import PlanPath -from static_centerline_generator.srv import PlanRoute +from autoware_static_centerline_generator.srv import LoadMap +from autoware_static_centerline_generator.srv import PlanPath +from autoware_static_centerline_generator.srv import PlanRoute rclpy.init() node = Node("static_centerline_generator_http_server") diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 7a5e215e2f975..fb8199f353b67 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp" int main(int argc, char * argv[]) { diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index e3b33a1adb875..c674e24695ffe 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" @@ -21,10 +21,12 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "static_centerline_generator/msg/points_with_lane_id.hpp" -#include "static_centerline_generator/type_alias.hpp" -#include "static_centerline_generator/utils.hpp" +#include "autoware_static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" +#include "autoware_static_centerline_generator/type_alias.hpp" +#include "autoware_static_centerline_generator/utils.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index ddfd3e11036ce..2ec1e7e8da325 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/utils.hpp" +#include "autoware_static_centerline_generator/utils.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" @@ -21,7 +21,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -228,4 +228,4 @@ MarkerArray create_distance_text_marker( return marker_array; } } // namespace utils -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv index 7d823b4eccbf9..3a8d0321ffb9a 100644 --- a/planning/static_centerline_generator/srv/PlanPath.srv +++ b/planning/static_centerline_generator/srv/PlanPath.srv @@ -1,6 +1,6 @@ uint32 map_id int64[] route --- -static_centerline_generator/PointsWithLaneId[] points_with_lane_ids +autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids string message int64[] unconnected_lane_ids diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 29ee49a11e3b3..52b7fd2d86c83 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,11 +28,11 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" + get_package_share_directory("autoware_static_centerline_generator"), "test/data/lanelet2_map.osm" ) static_centerline_generator_node = Node( - package="static_centerline_generator", + package="autoware_static_centerline_generator", executable="main", output="screen", parameters=[ @@ -50,7 +50,7 @@ def generate_test_description(): "mission_planner.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/static_centerline_generator.param.yaml", ), os.path.join( @@ -74,15 +74,15 @@ def generate_test_description(): "config/lanelet2_map_loader.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/vehicle_info.param.yaml", ), ], From 3493140846004e1c193ac74ffa678a1d2735649a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 2 Apr 2024 15:00:21 +0000 Subject: [PATCH 05/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../include/static_centerline_generator/type_alias.hpp | 6 +++--- planning/static_centerline_generator/scripts/app.py | 6 +++--- .../src/static_centerline_generator_node.cpp | 3 +++ .../test/test_static_centerline_generator.test.py | 3 ++- 4 files changed, 11 insertions(+), 7 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 8fddef2c28842..105d7f4a1eeb6 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ -#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#ifndef AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#define AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -43,4 +43,4 @@ using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#endif // AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index 7c40e1748d85a..08bff8b80dcb7 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -17,15 +17,15 @@ import json import uuid +from autoware_static_centerline_generator.srv import LoadMap +from autoware_static_centerline_generator.srv import PlanPath +from autoware_static_centerline_generator.srv import PlanRoute from flask import Flask from flask import jsonify from flask import request from flask_cors import CORS import rclpy from rclpy.node import Node -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute rclpy.init() node = Node("static_centerline_generator_http_server") diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index c674e24695ffe..325fc6e3e5909 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,9 @@ #include "autoware_static_centerline_generator/static_centerline_generator_node.hpp" +#include "autoware_static_centerline_optimizer/msg/points_with_lane_id.hpp" +#include "autoware_static_centerline_optimizer/type_alias.hpp" +#include "autoware_static_centerline_optimizer/utils.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 52b7fd2d86c83..2ecba10d994d7 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,7 +28,8 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), "test/data/lanelet2_map.osm" + get_package_share_directory("autoware_static_centerline_generator"), + "test/data/lanelet2_map.osm" ) static_centerline_generator_node = Node( From 3070fc9ce35f92023c104e555e18d1c312c832b9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 9 Apr 2024 14:55:07 +0200 Subject: [PATCH 06/33] build(autoware_static_centerline_generator): fix namespace Signed-off-by: Esteve Fernandez --- .../src/static_centerline_generator_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 325fc6e3e5909..8d8709a006b4e 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -59,7 +59,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -657,4 +657,4 @@ void StaticCenterlineGeneratorNode::save_map( lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", std::filesystem::copy_options::overwrite_existing); } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator From e6fde5945edbb9be1c2f41b5746472987e45d869 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 10 Apr 2024 12:47:28 +0200 Subject: [PATCH 07/33] fix(autoware_static_centerline_generator): fix clang-tidy issues Signed-off-by: Esteve Fernandez --- .../static_centerline_generator_node.hpp | 21 ++++++++++++++++++- .../static_centerline_generator/utils.hpp | 9 +++----- .../src/static_centerline_generator_node.cpp | 2 +- .../static_centerline_generator/src/utils.cpp | 6 +++--- 4 files changed, 27 insertions(+), 11 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 34ed92de182db..e76a5fca8cac0 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -34,11 +34,19 @@ #include #include +<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp namespace autoware::static_centerline_generator { using autoware::static_centerline_generator::srv::LoadMap; using autoware::static_centerline_generator::srv::PlanPath; using autoware::static_centerline_generator::srv::PlanRoute; +======= +namespace autoware::static_centerline_optimizer +{ +using autoware_static_centerline_optimizer::srv::LoadMap; +using autoware_static_centerline_optimizer::srv::PlanPath; +using autoware_static_centerline_optimizer::srv::PlanRoute; +>>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp struct CenterlineWithRoute { @@ -64,8 +72,14 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node void on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); +<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp // plan centerline CenterlineWithRoute generate_centerline_with_route(); +======= + // plan path + std::vector plan_path(const std::vector & route_lane_ids); + static std::vector optimize_trajectory(const Path & raw_path) ; +>>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -113,7 +127,12 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_; // vehicle info - vehicle_info_util::VehicleInfo vehicle_info_; + vehicle_info_util::VehicleInfo vehicle_info_{}; }; +<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp } // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +======= +} // namespace autoware::static_centerline_optimizer +#endif // AUTOWARE_STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ +>>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index a866d2a20b183..fc94fd698d721 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -24,9 +24,7 @@ #include #include -namespace autoware::static_centerline_generator -{ -namespace utils +namespace autoware::static_centerline_generator::utils { rclcpp::QoS create_transient_local_qos(); @@ -37,7 +35,7 @@ geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id); PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, + const RouteHandler & route_handler, const lanelet::ConstLanelets& lanelets, const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold, const double nearest_ego_yaw_threshold); @@ -52,7 +50,6 @@ MarkerArray create_footprint_marker( MarkerArray create_distance_text_marker( const geometry_msgs::msg::Pose & pose, const double dist, const std::array & marker_color, const rclcpp::Time & now, const size_t idx); -} // namespace utils -} // namespace autoware::static_centerline_generator +} // namespace autoware::static_centerline_generator::utils #endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 8d8709a006b4e..52315629f6c66 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -657,4 +657,4 @@ void StaticCenterlineGeneratorNode::save_map( lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", std::filesystem::copy_options::overwrite_existing); } -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index 2ec1e7e8da325..5c4adb8e34de0 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -32,7 +32,7 @@ nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs: return odometry_ptr; } -lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0) +lanelet::Point3d create_point3d(const double x, const double y, const double z = 19.0) { lanelet::Point3d point(lanelet::utils::getId()); point.setAttribute("local_x", x); @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose get_center_pose( } PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, + const RouteHandler & route_handler, const lanelet::ConstLanelets& lanelets, const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { @@ -141,7 +141,7 @@ void update_centerline( const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); if (is_inside) { - const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); + const auto center_point = create_point3d(traj_pos.x, traj_pos.y, traj_pos.z); // set center point centerline.push_back(center_point); From 9d734b2ba459570f800fe419c80d829b0c2bdb3a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Apr 2024 10:49:30 +0000 Subject: [PATCH 08/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../static_centerline_generator_node.hpp | 16 ---------------- .../static_centerline_generator/utils.hpp | 2 +- .../src/static_centerline_generator_node.cpp | 2 +- .../static_centerline_generator/src/utils.cpp | 2 +- 4 files changed, 3 insertions(+), 19 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index e76a5fca8cac0..092832bf980bd 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -34,19 +34,11 @@ #include #include -<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp namespace autoware::static_centerline_generator { using autoware::static_centerline_generator::srv::LoadMap; using autoware::static_centerline_generator::srv::PlanPath; using autoware::static_centerline_generator::srv::PlanRoute; -======= -namespace autoware::static_centerline_optimizer -{ -using autoware_static_centerline_optimizer::srv::LoadMap; -using autoware_static_centerline_optimizer::srv::PlanPath; -using autoware_static_centerline_optimizer::srv::PlanRoute; ->>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp struct CenterlineWithRoute { @@ -72,14 +64,11 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node void on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response); -<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp // plan centerline CenterlineWithRoute generate_centerline_with_route(); -======= // plan path std::vector plan_path(const std::vector & route_lane_ids); static std::vector optimize_trajectory(const Path & raw_path) ; ->>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -129,10 +118,5 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // vehicle info vehicle_info_util::VehicleInfo vehicle_info_{}; }; -<<<<<<< HEAD:planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp } // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -======= -} // namespace autoware::static_centerline_optimizer -#endif // AUTOWARE_STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ ->>>>>>> 5120827050 (fix(autoware_static_centerline_optimizer): fix clang-tidy issues):planning/static_centerline_optimizer/include/autoware_static_centerline_optimizer/static_centerline_optimizer_node.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index fc94fd698d721..b34caa11cec0d 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -35,7 +35,7 @@ geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id); PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets& lanelets, + const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold, const double nearest_ego_yaw_threshold); diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 52315629f6c66..8d8709a006b4e 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -657,4 +657,4 @@ void StaticCenterlineGeneratorNode::save_map( lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", std::filesystem::copy_options::overwrite_existing); } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index 5c4adb8e34de0..04c8f469200ce 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose get_center_pose( } PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets& lanelets, + const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { From 725f0bc9a4ade61dde78c4976c8a24743cfcf8cf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 10 Apr 2024 13:20:10 +0000 Subject: [PATCH 09/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../src/static_centerline_generator_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 8d8709a006b4e..0444c9abdeea5 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -32,6 +32,8 @@ #include "path_smoother/elastic_band_smoother.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" +#include "type_alias.hpp" +#include "utils.hpp" #include #include From 3c898f89afbbe2277ed8995ef898907c6f262db9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Fri, 12 Apr 2024 16:23:51 +0200 Subject: [PATCH 10/33] fix(autoware_static_centerline_generator): fix clang-tidy issues Signed-off-by: Esteve Fernandez --- .../static_centerline_generator_node.hpp | 1 + .../include/static_centerline_generator/type_alias.hpp | 2 -- .../include/static_centerline_generator/utils.hpp | 2 +- .../src/static_centerline_generator_node.cpp | 2 ++ 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 092832bf980bd..e8e5308d1655a 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -39,6 +39,7 @@ namespace autoware::static_centerline_generator using autoware::static_centerline_generator::srv::LoadMap; using autoware::static_centerline_generator::srv::PlanPath; using autoware::static_centerline_generator::srv::PlanRoute; +using ::route_handler::RouteHandler; struct CenterlineWithRoute { diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 105d7f4a1eeb6..d04117dda756b 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -36,10 +36,8 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; -using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; -using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index b34caa11cec0d..90b08c63e8715 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -36,7 +36,7 @@ geometry_msgs::msg::Pose get_center_pose( PathWithLaneId get_path_with_lane_id( const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, - const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold, + const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, const double nearest_ego_yaw_threshold); void update_centerline( diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 0444c9abdeea5..33fbde66424b2 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -63,6 +63,8 @@ namespace autoware::static_centerline_generator { +using ::tier4_autoware_utils::Point2d; + namespace { std::vector get_lane_ids_from_route(const LaneletRoute & route) From d4e2b59e945c4b665da6ad80959a85390daea11e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 15:05:01 +0200 Subject: [PATCH 11/33] fix(autoware_static_centerline_generator): fix build issues Signed-off-by: Esteve Fernandez --- .../src/static_centerline_generator_node.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 33fbde66424b2..cf0d2a084dcc4 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -32,8 +32,6 @@ #include "path_smoother/elastic_band_smoother.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -#include "type_alias.hpp" -#include "utils.hpp" #include #include From 7ac5a6786c9aee81a0fad3640ef5699f7f5c12cf Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 16:07:12 +0200 Subject: [PATCH 12/33] fix(autoware_static_centerline_generator): fix build issues Signed-off-by: Esteve Fernandez --- .../bag_ego_trajectory_based_centerline.hpp | 6 ++++-- .../optimization_trajectory_based_centerline.hpp | 12 ++++++++++-- .../static_centerline_generator_node.hpp | 10 +++++----- .../include/static_centerline_generator/utils.hpp | 4 +++- .../bag_ego_trajectory_based_centerline.cpp | 4 ++-- .../optimization_trajectory_based_centerline.cpp | 4 ++-- planning/static_centerline_generator/src/main.cpp | 2 +- .../src/static_centerline_generator_node.cpp | 11 ++++------- planning/static_centerline_generator/src/utils.cpp | 2 +- 9 files changed, 32 insertions(+), 23 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp index 30b2e55c36bba..d1c577457c145 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -17,11 +17,13 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_generator/type_alias.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { +using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 7e7cdea31e9f1..99037a56abdc2 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -16,12 +16,20 @@ #define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" +#include "route_handler/route_handler.hpp" #include "static_centerline_generator/type_alias.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/path.hpp" #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { +using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; +using ::autoware_auto_planning_msgs::msg::PathWithLaneId; +using ::route_handler::RouteHandler; +using ::autoware_auto_planning_msgs::msg::Path; class OptimizationTrajectoryBasedCenterline { public: @@ -37,7 +45,7 @@ class OptimizationTrajectoryBasedCenterline rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; }; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator // clang-format off #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT // clang-format on diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index e8e5308d1655a..306a0b495eacc 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -16,11 +16,11 @@ #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "autoware_static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" -#include "autoware_static_centerline_generator/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include @@ -36,9 +36,9 @@ namespace autoware::static_centerline_generator { -using autoware::static_centerline_generator::srv::LoadMap; -using autoware::static_centerline_generator::srv::PlanPath; -using autoware::static_centerline_generator::srv::PlanRoute; +using autoware_static_centerline_generator::srv::LoadMap; +using autoware_static_centerline_generator::srv::PlanPath; +using autoware_static_centerline_generator::srv::PlanRoute; using ::route_handler::RouteHandler; struct CenterlineWithRoute diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index 90b08c63e8715..1aeb471890253 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -16,7 +16,7 @@ #define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ #include "route_handler/route_handler.hpp" -#include "autoware_static_centerline_generator/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include @@ -26,6 +26,8 @@ namespace autoware::static_centerline_generator::utils { +using ::route_handler::RouteHandler; + rclcpp::QoS create_transient_local_qos(); lanelet::ConstLanelets get_lanelets_from_ids( diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 4e541b1aff527..48be274aa0543 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -20,7 +20,7 @@ #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node) { @@ -79,4 +79,4 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) return centerline_traj_points; } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 7980ae4e8d2d7..3c22740cd10d5 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -22,7 +22,7 @@ #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -180,4 +180,4 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra return whole_optimized_traj_points; } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index fb8199f353b67..7a5e215e2f975 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" int main(int argc, char * argv[]) { diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index cf0d2a084dcc4..2f78df235c79d 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -12,11 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" -#include "autoware_static_centerline_optimizer/msg/points_with_lane_id.hpp" -#include "autoware_static_centerline_optimizer/type_alias.hpp" -#include "autoware_static_centerline_optimizer/utils.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -24,10 +21,10 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "autoware_static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" -#include "autoware_static_centerline_generator/type_alias.hpp" -#include "autoware_static_centerline_generator/utils.hpp" +#include "static_centerline_generator/type_alias.hpp" +#include "static_centerline_generator/utils.hpp" #include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index 04c8f469200ce..5691dbec431d1 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_static_centerline_generator/utils.hpp" +#include "static_centerline_generator/utils.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" From 60553c671a80922d57ad10d56978141c766ec38e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 15 Apr 2024 14:09:17 +0000 Subject: [PATCH 13/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../bag_ego_trajectory_based_centerline.hpp | 1 + .../optimization_trajectory_based_centerline.hpp | 9 +++++---- .../static_centerline_generator_node.hpp | 6 +++--- .../include/static_centerline_generator/type_alias.hpp | 6 +++--- .../src/static_centerline_generator_node.cpp | 6 +++--- .../test/test_static_centerline_generator.test.py | 2 +- 6 files changed, 16 insertions(+), 14 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp index d1c577457c145..fb747c513eb68 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -17,6 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_generator/type_alias.hpp" + #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 99037a56abdc2..c0f0b19115a91 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -18,18 +18,19 @@ #include "rclcpp/rclcpp.hpp" #include "route_handler/route_handler.hpp" #include "static_centerline_generator/type_alias.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" + #include "autoware_auto_planning_msgs/msg/path.hpp" +#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" #include namespace autoware::static_centerline_generator { -using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; +using ::autoware_auto_planning_msgs::msg::Path; using ::autoware_auto_planning_msgs::msg::PathWithLaneId; +using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; using ::route_handler::RouteHandler; -using ::autoware_auto_planning_msgs::msg::Path; class OptimizationTrajectoryBasedCenterline { public: diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 306a0b495eacc..ec540c99f5919 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -15,11 +15,11 @@ #ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -69,7 +69,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node CenterlineWithRoute generate_centerline_with_route(); // plan path std::vector plan_path(const std::vector & route_lane_ids); - static std::vector optimize_trajectory(const Path & raw_path) ; + static std::vector optimize_trajectory(const Path & raw_path); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index d04117dda756b..e2e585c045002 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ -#define AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -41,4 +41,4 @@ using tier4_autoware_utils::LineString2d; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator -#endif // AUTOWARE_STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 2f78df235c79d..c5aba6e3d6362 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,7 @@ #include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -21,12 +22,11 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" +#include "obstacle_avoidance_planner/node.hpp" +#include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "static_centerline_generator/type_alias.hpp" #include "static_centerline_generator/utils.hpp" -#include "obstacle_avoidance_planner/node.hpp" -#include "path_smoother/elastic_band_smoother.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 2ecba10d994d7..0995bb9bb0424 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -29,7 +29,7 @@ def generate_test_description(): lanelet2_map_path = os.path.join( get_package_share_directory("autoware_static_centerline_generator"), - "test/data/lanelet2_map.osm" + "test/data/lanelet2_map.osm", ) static_centerline_generator_node = Node( From 77585575487ce0df4ab0faac38e39507c0234eea Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 15 Apr 2024 16:37:07 +0200 Subject: [PATCH 14/33] fix(autoware_static_centerline_optimizer): fix clang-tidy issues Signed-off-by: Esteve Fernandez --- .../optimization_trajectory_based_centerline.hpp | 2 +- .../include/static_centerline_generator/utils.hpp | 2 +- .../optimization_trajectory_based_centerline.cpp | 14 +++++++------- .../src/static_centerline_generator_node.cpp | 4 ++-- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index c0f0b19115a91..ba7a423e64984 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -41,7 +41,7 @@ class OptimizationTrajectoryBasedCenterline const std::vector & route_lane_ids); private: - std::vector optimize_trajectory(const Path & raw_path) const; + [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path) const; rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index 1aeb471890253..c11f1fde854fc 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -39,7 +39,7 @@ geometry_msgs::msg::Pose get_center_pose( PathWithLaneId get_path_with_lane_id( const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double nearest_ego_yaw_threshold); + const double ego_nearest_yaw_threshold); void update_centerline( RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 3c22740cd10d5..82ca5873f3ca2 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -79,13 +79,13 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); // get ego nearest search parameters and resample interval in behavior_path_planner - const double ego_nearest_dist_threshold = + const auto ego_nearest_dist_threshold = tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = + const auto ego_nearest_yaw_threshold = tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); - const double behavior_path_interval = + const auto behavior_path_interval = tier4_autoware_utils::getOrDeclareParameter(node, "output_path_interval"); - const double behavior_vel_interval = + const auto behavior_vel_interval = tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets @@ -107,7 +107,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( RCLCPP_INFO(node.get_logger(), "Converted to path and published."); // smooth trajectory and road collision avoidance - const auto optimized_traj_points = optimize_trajectory(raw_path); + auto optimized_traj_points = optimize_trajectory(raw_path); RCLCPP_INFO( node.get_logger(), "Smoothed trajectory and made it collision free with the road and published."); @@ -173,8 +173,8 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra break; } } - for (size_t j = 0; j < optimized_traj_points.size(); ++j) { - whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); + for (const auto & optimized_traj_point : optimized_traj_points) { + whole_optimized_traj_points.push_back(optimized_traj_point); } } diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index c5aba6e3d6362..8a3d162364e44 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -120,7 +120,7 @@ geometry_msgs::msg::Pose get_text_pose( std::array convert_hex_string_to_decimal(const std::string & hex_str_color) { - unsigned int hex_int_color; + unsigned int hex_int_color = 0.0; std::istringstream iss(hex_str_color); iss >> std::hex >> hex_int_color; @@ -213,7 +213,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( }); sub_traj_resample_interval_ = create_subscription( "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, - [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { + []([[maybe_unused]] const std_msgs::msg::Float32 & msg) { // TODO(murooka) }); From 96c1c69339943e5f2261fe73bc504668d7f243a4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 15 Apr 2024 15:17:28 +0000 Subject: [PATCH 15/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../optimization_trajectory_based_centerline.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index ba7a423e64984..67a70ca34c6bc 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -41,7 +41,8 @@ class OptimizationTrajectoryBasedCenterline const std::vector & route_lane_ids); private: - [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path) const; + [[nodiscard]] static std::vector optimize_trajectory( + const Path & raw_path) const; rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; From 9348cafec001d0d4a90d3468fcad46b120a909c6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 16 Apr 2024 15:18:01 +0200 Subject: [PATCH 16/33] build: fix build errors Signed-off-by: Esteve Fernandez --- .../optimization_trajectory_based_centerline.hpp | 3 +-- .../optimization_trajectory_based_centerline.cpp | 2 +- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 67a70ca34c6bc..6a2c834ef8d48 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -41,8 +41,7 @@ class OptimizationTrajectoryBasedCenterline const std::vector & route_lane_ids); private: - [[nodiscard]] static std::vector optimize_trajectory( - const Path & raw_path) const; + [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path); rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 82ca5873f3ca2..e379d8c0757ce 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -116,7 +116,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( } std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( - const Path & raw_path) const + const Path & raw_path) { // convert to trajectory points const auto raw_traj_points = [&]() { From 8953b3b4f21c638931bef8dde6d8cad2162a110b Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 16 Apr 2024 15:28:20 +0200 Subject: [PATCH 17/33] fix: remove else statements after return Signed-off-by: Esteve Fernandez --- .../src/static_centerline_generator_node.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 8a3d162364e44..15b0a172fc812 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -246,7 +246,8 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( if (centerline_source_param == "optimization_trajectory_base") { optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); return CenterlineSource::OptimizationTrajectoryBase; - } else if (centerline_source_param == "bag_ego_trajectory_base") { + } + if (centerline_source_param == "bag_ego_trajectory_base") { return CenterlineSource::BagEgoTrajectoryBase; } throw std::logic_error( @@ -305,7 +306,8 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; - } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + } + if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.front().pose); @@ -444,14 +446,14 @@ std::vector StaticCenterlineGeneratorNode::plan_route( mission_planner->initialize(&node, map_bin_ptr_); // plan route - const auto route = mission_planner->plan(check_points); + auto route = mission_planner->plan(check_points); return route; }(); RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets - const auto route_lane_ids = get_lane_ids_from_route(route); + auto route_lane_ids = get_lane_ids_from_route(route); return route_lane_ids; } From 1b9ad21bb05383249caedec75bbdd4dbd5f09355 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Tue, 16 Apr 2024 16:13:31 +0200 Subject: [PATCH 18/33] fix(autoware_static_centerline_generator): fix clang-tidy issues Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 18 +- ...timization_trajectory_based_centerline.hpp | 6 +- .../static_centerline_generator_node.hpp | 6 +- .../static_centerline_generator.launch.xml | 2 +- .../bag_ego_trajectory_based_centerline.cpp | 5 +- ...timization_trajectory_based_centerline.cpp | 41 ++-- .../static_centerline_generator/src/main.cpp | 40 ---- .../src/static_centerline_generator_node.cpp | 176 ++++++++++-------- .../static_centerline_generator/src/utils.cpp | 19 +- 9 files changed, 153 insertions(+), 160 deletions(-) delete mode 100644 planning/static_centerline_generator/src/main.cpp diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index 3993674c89e8c..6166f49847de8 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - autoware_static_centerline_generator + ${PROJECT_NAME} "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" @@ -19,21 +19,25 @@ rosidl_generate_interfaces( autoware_package() -ament_auto_add_executable(main - src/main.cpp +ament_auto_add_library(${PROJECT_NAME}_node SHARED src/static_centerline_generator_node.cpp src/centerline_source/optimization_trajectory_based_centerline.cpp src/centerline_source/bag_ego_trajectory_based_centerline.cpp src/utils.cpp ) +rclcpp_components_register_node(${PROJECT_NAME}_node + PLUGIN "autoware::static_centerline_generator::StaticCenterlineGeneratorNode" + EXECUTABLE ${PROJECT_NAME}_exe +) + if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(main - autoware_static_centerline_generator "rosidl_typesupport_cpp") + rosidl_target_interfaces(${PROJECT_NAME}_node + ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") - target_link_libraries(main "${cpp_typesupport_target}") + cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") + target_link_libraries(${PROJECT_NAME}_node "${cpp_typesupport_target}") endif() ament_auto_package( diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 6a2c834ef8d48..711e3b56cc464 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -12,8 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#ifndef \ + STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define \ + STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" #include "route_handler/route_handler.hpp" diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index ec540c99f5919..041352b4b71e9 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -19,7 +19,8 @@ #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" #include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include \ + "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -89,7 +90,8 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node std::pair traj_range_indices_{0, 0}; std::optional centerline_with_route_{std::nullopt}; - enum class CenterlineSource { + enum class CenterlineSource + { OptimizationTrajectoryBase = 0, BagEgoTrajectoryBase, }; diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index 8c05f9a960d0f..eb068cbd32bab 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 48be274aa0543..d50f9c11492bc 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -49,9 +49,10 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) constexpr double epsilon = 1e-1; if ( std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < - epsilon && + epsilon && std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < - epsilon) { + epsilon) + { continue; } } diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index e379d8c0757ce..9d4e4910b405c 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -12,7 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include \ + "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" @@ -90,19 +91,19 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); - }(); + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); // convert path with lane id to path const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(node.get_logger(), "Converted to path and published."); @@ -120,9 +121,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra { // convert to trajectory points const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); // create an instance of elastic band and model predictive trajectory. const auto eb_path_smoother_ptr = @@ -132,7 +133,8 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; + const int traj_segment_num = static_cast(raw_traj_points.size()) / + valid_optimized_traj_points_num; // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use // warm start. @@ -140,15 +142,16 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra std::vector whole_optimized_traj_points; for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) + { // calculate virtual ego pose for the optimization constexpr int virtual_ego_pose_offset_idx = 1; const auto virtual_ego_pose = raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; // smooth trajectory by elastic band in the path_smoother package const auto smoothed_traj_points = @@ -168,7 +171,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra if (dist < 0.5) { const std::vector extracted_whole_optimized_traj_points{ whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; + whole_optimized_traj_points.begin() + static_cast(std::max(j, 1UL)) - 1}; whole_optimized_traj_points = extracted_whole_optimized_traj_points; break; } diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp deleted file mode 100644 index 7a5e215e2f975..0000000000000 --- a/planning/static_centerline_generator/src/main.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2022 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "static_centerline_generator/static_centerline_generator_node.hpp" - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - - // initialize node - rclcpp::NodeOptions node_options; - auto node = - std::make_shared( - node_options); - - // get ros parameter - const bool run_background = node->declare_parameter("run_background"); - - // process - if (!run_background) { - node->run(); - } - - // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 15b0a172fc812..085a5f3494f27 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -141,7 +141,8 @@ std::vector check_lanelet_connection( const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); const bool is_connected = - std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { + std::find_if( + next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { return next_lanelet.id() == route_lanelets.at(i + 1).id(); }) != next_lanelets.end(); if (!is_connected) { @@ -200,8 +201,8 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { const auto lanelet2_output_file_path = - tier4_autoware_utils::getOrDeclareParameter( - *this, "lanelet2_output_file_path"); + tier4_autoware_utils::getOrDeclareParameter( + *this, "lanelet2_output_file_path"); if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; const auto selected_centerline = std::vector( @@ -242,17 +243,17 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); centerline_source_ = [&]() { - const auto centerline_source_param = declare_parameter("centerline_source"); - if (centerline_source_param == "optimization_trajectory_base") { - optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); - return CenterlineSource::OptimizationTrajectoryBase; - } - if (centerline_source_param == "bag_ego_trajectory_base") { - return CenterlineSource::BagEgoTrajectoryBase; - } - throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); - }(); + const auto centerline_source_param = declare_parameter("centerline_source"); + if (centerline_source_param == "optimization_trajectory_base") { + optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); + return CenterlineSource::OptimizationTrajectoryBase; + } + if (centerline_source_param == "bag_ego_trajectory_base") { + return CenterlineSource::BagEgoTrajectoryBase; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); } void StaticCenterlineGeneratorNode::update_centerline_range( @@ -298,12 +299,12 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout // generate centerline with route auto centerline_with_route = [&]() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto optimized_centerline = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto optimized_centerline = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } @@ -317,27 +318,39 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); return CenterlineWithRoute{}; } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - - return CenterlineWithRoute{bag_centerline, route_lane_ids}; - } - throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); - }(); + if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + const auto bag_centerline = generate_centerline_with_bag(*this); + const auto start_lanelets = + route_handler_ptr_->getClosestLanelets(bag_centerline.front().pose); + const auto end_lanelets = + route_handler_ptr_->getClosestLanelets(bag_centerline.back().pose); + if (start_lanelets.empty() || end_lanelets.empty()) { + RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); + return CenterlineWithRoute{}; + } + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + + return CenterlineWithRoute{bag_centerline, route_lane_ids}; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); // resample const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); centerline_with_route.centerline = resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - pub_whole_centerline_->publish(motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); + pub_whole_centerline_->publish( + motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish(motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); + pub_centerline_->publish( + motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); return centerline_with_route; } @@ -353,32 +366,32 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { - // load map - const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); - const auto map_ptr = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); - if (!map_ptr) { - return nullptr; - } + // load map + const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); + const auto map_ptr = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + if (!map_ptr) { + return nullptr; + } - // NOTE: generate map projector for lanelet::write(). - // Without this, lat/lon of the generated LL2 map will be wrong. - map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); + // NOTE: generate map projector for lanelet::write(). + // Without this, lat/lon of the generated LL2 map will be wrong. + map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); - // NOTE: The original map is stored here since the various ids in the lanelet map will change - // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. - original_map_ptr_ = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + // NOTE: The original map is stored here since the various ids in the lanelet map will change + // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. + original_map_ptr_ = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); - // overwrite more dense centerline - lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); + // overwrite more dense centerline + lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); - // create map bin msg - const auto map_bin_msg = - Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); + // create map bin msg + const auto map_bin_msg = + Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); - return std::make_shared(map_bin_msg); - }(); + return std::make_shared(map_bin_msg); + }(); // check if map_bin_ptr_ is not null pointer if (!map_bin_ptr_) { @@ -427,29 +440,29 @@ std::vector StaticCenterlineGeneratorNode::plan_route( // calculate check points (= start and goal pose) const auto check_points = [&]() { - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return std::vector{start_center_pose, end_center_pose}; - }(); + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return std::vector{start_center_pose, end_center_pose}; + }(); RCLCPP_INFO(get_logger(), "Calculated check points."); // plan route by the mission_planner package const auto route = [&]() { - // create mission_planner plugin - auto plugin_loader = pluginlib::ClassLoader( - "mission_planner", "mission_planner::PlannerPlugin"); - auto mission_planner = - plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); + // create mission_planner plugin + auto plugin_loader = pluginlib::ClassLoader( + "mission_planner", "mission_planner::PlannerPlugin"); + auto mission_planner = + plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); - // initialize mission_planner - auto node = rclcpp::Node("mission_planner"); - mission_planner->initialize(&node, map_bin_ptr_); + // initialize mission_planner + auto node = rclcpp::Node("mission_planner"); + mission_planner->initialize(&node, map_bin_ptr_); - // plan route - auto route = mission_planner->plan(check_points); + // plan route + auto route = mission_planner->plan(check_points); - return route; - }(); + return route; + }(); RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets @@ -515,7 +528,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( // plan path const auto optimized_traj_points = optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); + *this, *route_handler_ptr_, route_lane_ids); // check calculation result if (optimized_traj_points.empty()) { @@ -535,7 +548,8 @@ void StaticCenterlineGeneratorNode::on_plan_path( // check if target point is inside the lanelet while (lanelet::geometry::inside( - lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { + lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) + { // memorize points inside the lanelet current_lanelet_points.push_back(target_traj_point->pose.position); target_traj_point++; @@ -573,14 +587,14 @@ void StaticCenterlineGeneratorNode::evaluate( const auto marker_color_vec = tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { - for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { - const double dist_thresh = dist_thresh_vec.at(i); - if (dist < dist_thresh) { - return convert_hex_string_to_decimal(marker_color_vec.at(i)); + for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { + const double dist_thresh = dist_thresh_vec.at(i); + if (dist < dist_thresh) { + return convert_hex_string_to_decimal(marker_color_vec.at(i)); + } } - } - return boost::none; - }; + return boost::none; + }; // create right/left bound LineString2d right_bound; @@ -659,3 +673,5 @@ void StaticCenterlineGeneratorNode::save_map( std::filesystem::copy_options::overwrite_existing); } } // namespace autoware::static_centerline_generator +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::static_centerline_generator::StaticCenterlineGeneratorNode) diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index 5691dbec431d1..c28d7a8c10bfd 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -65,9 +65,9 @@ geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id) { // get middle idx of the lanelet - const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); + const auto lanelet = route_handler.getLaneletsFromId(static_cast(lanelet_id)); const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(center_line.size() / 2.0); + const size_t middle_point_idx = std::floor(static_cast(center_line.size()) / 2.0); // get middle position of the lanelet geometry_msgs::msg::Point middle_pos; @@ -110,7 +110,8 @@ PathWithLaneId get_path_with_lane_id( constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); + path_with_lane_id, drivable_lanes, false, false, static_cast(vehicle_length), + planner_data); return path_with_lane_id; } @@ -178,10 +179,12 @@ MarkerArray create_footprint_marker( const double b = marker_color.at(2); auto marker = tier4_autoware_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints", idx, + "map", rclcpp::Clock().now(), "unsafe_footprints", static_cast(idx), visualization_msgs::msg::Marker::LINE_STRIP, tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + tier4_autoware_utils::createMarkerColor( + static_cast(r), static_cast(g), + static_cast(b), 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -210,10 +213,12 @@ MarkerArray create_distance_text_marker( const double b = marker_color.at(2); auto marker = tier4_autoware_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, + "map", rclcpp::Clock().now(), "unsafe_footprints_distance", static_cast(idx), visualization_msgs::msg::Marker::TEXT_VIEW_FACING, tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); + tier4_autoware_utils::createMarkerColor( + static_cast(r), static_cast(g), + static_cast(b), 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); From 41afbc8c8baaed0fb15338e7a5c529e36982b3cb Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 16 Apr 2024 14:21:38 +0000 Subject: [PATCH 19/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- ...timization_trajectory_based_centerline.hpp | 44 ++--- .../static_centerline_generator_node.hpp | 6 +- .../bag_ego_trajectory_based_centerline.cpp | 5 +- ...timization_trajectory_based_centerline.cpp | 40 ++-- .../src/static_centerline_generator_node.cpp | 177 ++++++++---------- .../static_centerline_generator/src/utils.cpp | 6 +- 6 files changed, 129 insertions(+), 149 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 711e3b56cc464..09834bf21c75a 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef \ - STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define \ - STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ +STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ + STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" #include "route_handler/route_handler.hpp" @@ -27,27 +27,27 @@ #include -namespace autoware::static_centerline_generator + namespace autoware::static_centerline_generator { -using ::autoware_auto_planning_msgs::msg::Path; -using ::autoware_auto_planning_msgs::msg::PathWithLaneId; -using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; -using ::route_handler::RouteHandler; -class OptimizationTrajectoryBasedCenterline -{ -public: - OptimizationTrajectoryBasedCenterline() = default; - explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); - std::vector generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids); + using ::autoware_auto_planning_msgs::msg::Path; + using ::autoware_auto_planning_msgs::msg::PathWithLaneId; + using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; + using ::route_handler::RouteHandler; + class OptimizationTrajectoryBasedCenterline + { + public: + OptimizationTrajectoryBasedCenterline() = default; + explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); + std::vector generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids); -private: - [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path); + private: + [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path); - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; -}; + rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; + rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; + }; } // namespace autoware::static_centerline_generator // clang-format off #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 041352b4b71e9..ec540c99f5919 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -19,8 +19,7 @@ #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" #include "rclcpp/rclcpp.hpp" -#include \ - "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -90,8 +89,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node std::pair traj_range_indices_{0, 0}; std::optional centerline_with_route_{std::nullopt}; - enum class CenterlineSource - { + enum class CenterlineSource { OptimizationTrajectoryBase = 0, BagEgoTrajectoryBase, }; diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index d50f9c11492bc..48be274aa0543 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -49,10 +49,9 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) constexpr double epsilon = 1e-1; if ( std::abs(centerline_traj_points.back().pose.position.x - ros_msg->pose.pose.position.x) < - epsilon && + epsilon && std::abs(centerline_traj_points.back().pose.position.y - ros_msg->pose.pose.position.y) < - epsilon) - { + epsilon) { continue; } } diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 9d4e4910b405c..1c79bccab5e79 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -12,8 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include \ - "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" @@ -91,19 +90,19 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( // extract path with lane id from lanelets const auto raw_path_with_lane_id = [&]() { - const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( - route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, - ego_nearest_yaw_threshold); - return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); - }(); + const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id( + route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold, + ego_nearest_yaw_threshold); + return motion_utils::resamplePath(non_resampled_path_with_lane_id, behavior_path_interval); + }(); pub_raw_path_with_lane_id_->publish(raw_path_with_lane_id); RCLCPP_INFO(node.get_logger(), "Calculated raw path with lane id and published."); // convert path with lane id to path const auto raw_path = [&]() { - const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); - return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); - }(); + const auto non_resampled_path = convert_to_path(raw_path_with_lane_id); + return motion_utils::resamplePath(non_resampled_path, behavior_vel_interval); + }(); pub_raw_path_->publish(raw_path); RCLCPP_INFO(node.get_logger(), "Converted to path and published."); @@ -121,9 +120,9 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra { // convert to trajectory points const auto raw_traj_points = [&]() { - const auto raw_traj = convert_to_trajectory(raw_path); - return motion_utils::convertToTrajectoryPointArray(raw_traj); - }(); + const auto raw_traj = convert_to_trajectory(raw_path); + return motion_utils::convertToTrajectoryPointArray(raw_traj); + }(); // create an instance of elastic band and model predictive trajectory. const auto eb_path_smoother_ptr = @@ -133,8 +132,8 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = static_cast(raw_traj_points.size()) / - valid_optimized_traj_points_num; + const int traj_segment_num = + static_cast(raw_traj_points.size()) / valid_optimized_traj_points_num; // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use // warm start. @@ -142,16 +141,15 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra std::vector whole_optimized_traj_points; for (int virtual_ego_pose_idx = -num_initial_optimization; - virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) - { + virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) { // calculate virtual ego pose for the optimization constexpr int virtual_ego_pose_offset_idx = 1; const auto virtual_ego_pose = raw_traj_points - .at( - valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + - virtual_ego_pose_offset_idx) - .pose; + .at( + valid_optimized_traj_points_num * std::max(virtual_ego_pose_idx, 0) + + virtual_ego_pose_offset_idx) + .pose; // smooth trajectory by elastic band in the path_smoother package const auto smoothed_traj_points = diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 085a5f3494f27..9f538b418421b 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -141,8 +141,7 @@ std::vector check_lanelet_connection( const auto next_lanelets = route_handler.getNextLanelets(route_lanelets.at(i)); const bool is_connected = - std::find_if( - next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { + std::find_if(next_lanelets.begin(), next_lanelets.end(), [&](const auto & next_lanelet) { return next_lanelet.id() == route_lanelets.at(i + 1).id(); }) != next_lanelets.end(); if (!is_connected) { @@ -201,8 +200,8 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( sub_save_map_ = create_subscription( "/centerline_updater_helper/save_map", rclcpp::QoS{1}, [this](const std_msgs::msg::Bool & msg) { const auto lanelet2_output_file_path = - tier4_autoware_utils::getOrDeclareParameter( - *this, "lanelet2_output_file_path"); + tier4_autoware_utils::getOrDeclareParameter( + *this, "lanelet2_output_file_path"); if (!centerline_with_route_ || msg.data) { const auto & c = *centerline_with_route_; const auto selected_centerline = std::vector( @@ -243,17 +242,17 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( vehicle_info_ = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); centerline_source_ = [&]() { - const auto centerline_source_param = declare_parameter("centerline_source"); - if (centerline_source_param == "optimization_trajectory_base") { - optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); - return CenterlineSource::OptimizationTrajectoryBase; - } - if (centerline_source_param == "bag_ego_trajectory_base") { - return CenterlineSource::BagEgoTrajectoryBase; - } - throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); - }(); + const auto centerline_source_param = declare_parameter("centerline_source"); + if (centerline_source_param == "optimization_trajectory_base") { + optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); + return CenterlineSource::OptimizationTrajectoryBase; + } + if (centerline_source_param == "bag_ego_trajectory_base") { + return CenterlineSource::BagEgoTrajectoryBase; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); } void StaticCenterlineGeneratorNode::update_centerline_range( @@ -299,12 +298,12 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout // generate centerline with route auto centerline_with_route = [&]() { - if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { - const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); - const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - const auto optimized_centerline = - optimization_trajectory_based_centerline_.generate_centerline_with_optimization( + if (centerline_source_ == CenterlineSource::OptimizationTrajectoryBase) { + const lanelet::Id start_lanelet_id = declare_parameter("start_lanelet_id"); + const lanelet::Id end_lanelet_id = declare_parameter("end_lanelet_id"); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + const auto optimized_centerline = + optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; } @@ -318,39 +317,27 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); return CenterlineWithRoute{}; } - if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { - const auto bag_centerline = generate_centerline_with_bag(*this); - const auto start_lanelets = - route_handler_ptr_->getClosestLanelets(bag_centerline.front().pose); - const auto end_lanelets = - route_handler_ptr_->getClosestLanelets(bag_centerline.back().pose); - if (start_lanelets.empty() || end_lanelets.empty()) { - RCLCPP_ERROR(get_logger(), "Nearest lanelets to the bag's centerline are not found."); - return CenterlineWithRoute{}; - } - - const lanelet::Id start_lanelet_id = start_lanelets.front().id(); - const lanelet::Id end_lanelet_id = end_lanelets.front().id(); - const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); - - return CenterlineWithRoute{bag_centerline, route_lane_ids}; - } - throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); - }(); + + const lanelet::Id start_lanelet_id = start_lanelets.front().id(); + const lanelet::Id end_lanelet_id = end_lanelets.front().id(); + const auto route_lane_ids = plan_route(start_lanelet_id, end_lanelet_id); + + return CenterlineWithRoute{bag_centerline, route_lane_ids}; + } + throw std::logic_error( + "The centerline source is not supported in static_centerline_generator."); + }(); // resample const double output_trajectory_interval = declare_parameter("output_trajectory_interval"); centerline_with_route.centerline = resample_trajectory_points(centerline_with_route.centerline, output_trajectory_interval); - pub_whole_centerline_->publish( - motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); + pub_whole_centerline_->publish(motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); - pub_centerline_->publish( - motion_utils::convertToTrajectory( - centerline_with_route.centerline, create_header(this->now()))); + pub_centerline_->publish(motion_utils::convertToTrajectory( + centerline_with_route.centerline, create_header(this->now()))); return centerline_with_route; } @@ -366,32 +353,32 @@ void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_ // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { - // load map - const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); - const auto map_ptr = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); - if (!map_ptr) { - return nullptr; - } + // load map + const auto map_projector_info = load_info_from_lanelet2_map(lanelet2_input_file_path); + const auto map_ptr = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + if (!map_ptr) { + return nullptr; + } - // NOTE: generate map projector for lanelet::write(). - // Without this, lat/lon of the generated LL2 map will be wrong. - map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); + // NOTE: generate map projector for lanelet::write(). + // Without this, lat/lon of the generated LL2 map will be wrong. + map_projector_ = geography_utils::get_lanelet2_projector(map_projector_info); - // NOTE: The original map is stored here since the various ids in the lanelet map will change - // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. - original_map_ptr_ = - Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); + // NOTE: The original map is stored here since the various ids in the lanelet map will change + // after lanelet::utils::overwriteLaneletCenterline, and saving map will fail. + original_map_ptr_ = + Lanelet2MapLoaderNode::load_map(lanelet2_input_file_path, map_projector_info); - // overwrite more dense centerline - lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); + // overwrite more dense centerline + lanelet::utils::overwriteLaneletsCenterline(map_ptr, 5.0, false); - // create map bin msg - const auto map_bin_msg = - Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); + // create map bin msg + const auto map_bin_msg = + Lanelet2MapLoaderNode::create_map_bin_msg(map_ptr, lanelet2_input_file_path, now()); - return std::make_shared(map_bin_msg); - }(); + return std::make_shared(map_bin_msg); + }(); // check if map_bin_ptr_ is not null pointer if (!map_bin_ptr_) { @@ -440,29 +427,29 @@ std::vector StaticCenterlineGeneratorNode::plan_route( // calculate check points (= start and goal pose) const auto check_points = [&]() { - const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); - const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); - return std::vector{start_center_pose, end_center_pose}; - }(); + const auto start_center_pose = utils::get_center_pose(*route_handler_ptr_, start_lanelet_id); + const auto end_center_pose = utils::get_center_pose(*route_handler_ptr_, end_lanelet_id); + return std::vector{start_center_pose, end_center_pose}; + }(); RCLCPP_INFO(get_logger(), "Calculated check points."); // plan route by the mission_planner package const auto route = [&]() { - // create mission_planner plugin - auto plugin_loader = pluginlib::ClassLoader( - "mission_planner", "mission_planner::PlannerPlugin"); - auto mission_planner = - plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); + // create mission_planner plugin + auto plugin_loader = pluginlib::ClassLoader( + "mission_planner", "mission_planner::PlannerPlugin"); + auto mission_planner = + plugin_loader.createSharedInstance("mission_planner::lanelet2::DefaultPlanner"); - // initialize mission_planner - auto node = rclcpp::Node("mission_planner"); - mission_planner->initialize(&node, map_bin_ptr_); + // initialize mission_planner + auto node = rclcpp::Node("mission_planner"); + mission_planner->initialize(&node, map_bin_ptr_); - // plan route - auto route = mission_planner->plan(check_points); + // plan route + auto route = mission_planner->plan(check_points); - return route; - }(); + return route; + }(); RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets @@ -528,7 +515,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( // plan path const auto optimized_traj_points = optimization_trajectory_based_centerline_.generate_centerline_with_optimization( - *this, *route_handler_ptr_, route_lane_ids); + *this, *route_handler_ptr_, route_lane_ids); // check calculation result if (optimized_traj_points.empty()) { @@ -548,8 +535,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( // check if target point is inside the lanelet while (lanelet::geometry::inside( - lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) - { + lanelet, convert_to_lanelet_point(target_traj_point->pose.position))) { // memorize points inside the lanelet current_lanelet_points.push_back(target_traj_point->pose.position); target_traj_point++; @@ -587,14 +573,14 @@ void StaticCenterlineGeneratorNode::evaluate( const auto marker_color_vec = tier4_autoware_utils::getOrDeclareParameter>(*this, "marker_color"); const auto get_marker_color = [&](const double dist) -> boost::optional> { - for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { - const double dist_thresh = dist_thresh_vec.at(i); - if (dist < dist_thresh) { - return convert_hex_string_to_decimal(marker_color_vec.at(i)); - } + for (size_t i = 0; i < dist_thresh_vec.size(); ++i) { + const double dist_thresh = dist_thresh_vec.at(i); + if (dist < dist_thresh) { + return convert_hex_string_to_decimal(marker_color_vec.at(i)); } - return boost::none; - }; + } + return boost::none; + }; // create right/left bound LineString2d right_bound; @@ -674,4 +660,5 @@ void StaticCenterlineGeneratorNode::save_map( } } // namespace autoware::static_centerline_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(autoware::static_centerline_generator::StaticCenterlineGeneratorNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::static_centerline_generator::StaticCenterlineGeneratorNode) diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index c28d7a8c10bfd..6aba7d5651519 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -183,8 +183,7 @@ MarkerArray create_footprint_marker( visualization_msgs::msg::Marker::LINE_STRIP, tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), tier4_autoware_utils::createMarkerColor( - static_cast(r), static_cast(g), - static_cast(b), 0.999)); + static_cast(r), static_cast(g), static_cast(b), 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -217,8 +216,7 @@ MarkerArray create_distance_text_marker( visualization_msgs::msg::Marker::TEXT_VIEW_FACING, tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), tier4_autoware_utils::createMarkerColor( - static_cast(r), static_cast(g), - static_cast(b), 0.999)); + static_cast(r), static_cast(g), static_cast(b), 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); From a49d9cdde0610d06647ea87df8a186a03430da2e Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 18 Apr 2024 15:33:23 +0200 Subject: [PATCH 20/33] revert changes for static_centerline_generator Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 20 ++++----- .../static_centerline_generator/README.md | 4 +- .../bag_ego_trajectory_based_centerline.hpp | 7 +-- ...timization_trajectory_based_centerline.hpp | 45 +++++++------------ .../static_centerline_generator_node.hpp | 22 ++++----- .../type_alias.hpp | 6 ++- .../static_centerline_generator/utils.hpp | 15 ++++--- .../launch/run_planning_server.launch.xml | 4 +- .../static_centerline_generator.launch.xml | 6 +-- .../static_centerline_generator/package.xml | 4 +- .../scripts/app.py | 6 +-- .../bag_ego_trajectory_based_centerline.cpp | 4 +- ...timization_trajectory_based_centerline.cpp | 25 +++++------ .../static_centerline_generator/src/main.cpp | 39 ++++++++++++++++ .../src/static_centerline_generator_node.cpp | 29 +++++------- .../static_centerline_generator/src/utils.cpp | 27 +++++------ .../srv/PlanPath.srv | 2 +- .../test_static_centerline_generator.test.py | 13 +++--- 18 files changed, 142 insertions(+), 136 deletions(-) create mode 100644 planning/static_centerline_generator/src/main.cpp diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index 6166f49847de8..991d12097cc08 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(autoware_static_centerline_generator) +project(static_centerline_generator) find_package(autoware_cmake REQUIRED) @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - ${PROJECT_NAME} + static_centerline_generator "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" @@ -19,25 +19,21 @@ rosidl_generate_interfaces( autoware_package() -ament_auto_add_library(${PROJECT_NAME}_node SHARED +ament_auto_add_executable(main + src/main.cpp src/static_centerline_generator_node.cpp src/centerline_source/optimization_trajectory_based_centerline.cpp src/centerline_source/bag_ego_trajectory_based_centerline.cpp src/utils.cpp ) -rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "autoware::static_centerline_generator::StaticCenterlineGeneratorNode" - EXECUTABLE ${PROJECT_NAME}_exe -) - if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(${PROJECT_NAME}_node - ${PROJECT_NAME} "rosidl_typesupport_cpp") + rosidl_target_interfaces(main + static_centerline_generator "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(${PROJECT_NAME}_node "${cpp_typesupport_target}") + cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp") + target_link_libraries(main "${cpp_typesupport_target}") endif() ament_auto_package( diff --git a/planning/static_centerline_generator/README.md b/planning/static_centerline_generator/README.md index 844d0af15f2a5..00572b754645c 100644 --- a/planning/static_centerline_generator/README.md +++ b/planning/static_centerline_generator/README.md @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= +ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:= ``` FYI, port ID of the http server is 4010 by default. @@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des - `` ```sh -ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= ``` The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp index fb747c513eb68..30b2e55c36bba 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -18,13 +18,10 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_generator/type_alias.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - #include -namespace autoware::static_centerline_generator +namespace static_centerline_generator { -using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 09834bf21c75a..7e7cdea31e9f1 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -12,43 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ -STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ - STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" -#include "route_handler/route_handler.hpp" #include "static_centerline_generator/type_alias.hpp" -#include "autoware_auto_planning_msgs/msg/path.hpp" -#include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" - #include - namespace autoware::static_centerline_generator +namespace static_centerline_generator +{ +class OptimizationTrajectoryBasedCenterline { - using ::autoware_auto_planning_msgs::msg::Path; - using ::autoware_auto_planning_msgs::msg::PathWithLaneId; - using ::autoware_auto_planning_msgs::msg::TrajectoryPoint; - using ::route_handler::RouteHandler; - class OptimizationTrajectoryBasedCenterline - { - public: - OptimizationTrajectoryBasedCenterline() = default; - explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); - std::vector generate_centerline_with_optimization( - rclcpp::Node & node, const RouteHandler & route_handler, - const std::vector & route_lane_ids); +public: + OptimizationTrajectoryBasedCenterline() = default; + explicit OptimizationTrajectoryBasedCenterline(rclcpp::Node & node); + std::vector generate_centerline_with_optimization( + rclcpp::Node & node, const RouteHandler & route_handler, + const std::vector & route_lane_ids); - private: - [[nodiscard]] static std::vector optimize_trajectory(const Path & raw_path); +private: + std::vector optimize_trajectory(const Path & raw_path) const; - rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; - rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; - }; -} // namespace autoware::static_centerline_generator + rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; + rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; +}; +} // namespace static_centerline_generator // clang-format off #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT // clang-format on diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index ec540c99f5919..c1d92c06a8e05 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -15,11 +15,11 @@ #ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#include "autoware_static_centerline_generator/srv/load_map.hpp" -#include "autoware_static_centerline_generator/srv/plan_path.hpp" -#include "autoware_static_centerline_generator/srv/plan_route.hpp" #include "rclcpp/rclcpp.hpp" #include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "static_centerline_generator/srv/load_map.hpp" +#include "static_centerline_generator/srv/plan_path.hpp" +#include "static_centerline_generator/srv/plan_route.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -34,12 +34,11 @@ #include #include -namespace autoware::static_centerline_generator +namespace static_centerline_generator { -using autoware_static_centerline_generator::srv::LoadMap; -using autoware_static_centerline_generator::srv::PlanPath; -using autoware_static_centerline_generator::srv::PlanRoute; -using ::route_handler::RouteHandler; +using static_centerline_generator::srv::LoadMap; +using static_centerline_generator::srv::PlanPath; +using static_centerline_generator::srv::PlanRoute; struct CenterlineWithRoute { @@ -67,9 +66,6 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // plan centerline CenterlineWithRoute generate_centerline_with_route(); - // plan path - std::vector plan_path(const std::vector & route_lane_ids); - static std::vector optimize_trajectory(const Path & raw_path); void on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response); @@ -117,7 +113,7 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr callback_group_; // vehicle info - vehicle_info_util::VehicleInfo vehicle_info_{}; + vehicle_info_util::VehicleInfo vehicle_info_; }; -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index e2e585c045002..2dcb9cbbd099f 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -26,7 +26,7 @@ #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware::static_centerline_generator +namespace static_centerline_generator { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -36,9 +36,11 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; using autoware_planning_msgs::msg::LaneletRoute; +using route_handler::RouteHandler; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index c11f1fde854fc..c8cf8f8b90590 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -24,10 +24,10 @@ #include #include -namespace autoware::static_centerline_generator::utils +namespace static_centerline_generator +{ +namespace utils { -using ::route_handler::RouteHandler; - rclcpp::QoS create_transient_local_qos(); lanelet::ConstLanelets get_lanelets_from_ids( @@ -37,9 +37,9 @@ geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id); PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, - const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, - const double ego_nearest_yaw_threshold); + const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, + const geometry_msgs::msg::Pose & start_pose, const double nearest_ego_dist_threshold, + const double nearest_ego_yaw_threshold); void update_centerline( RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, @@ -52,6 +52,7 @@ MarkerArray create_footprint_marker( MarkerArray create_distance_text_marker( const geometry_msgs::msg::Pose & pose, const double dist, const std::array & marker_color, const rclcpp::Time & now, const size_t idx); -} // namespace autoware::static_centerline_generator::utils +} // namespace utils +} // namespace static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_generator/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml index 6ce0054343acc..1493078317458 100644 --- a/planning/static_centerline_generator/launch/run_planning_server.launch.xml +++ b/planning/static_centerline_generator/launch/run_planning_server.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index eb068cbd32bab..ae71b0ae6e925 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + @@ -79,11 +79,11 @@ - + - + diff --git a/planning/static_centerline_generator/package.xml b/planning/static_centerline_generator/package.xml index 96e17595d49ee..6573f6e439c43 100644 --- a/planning/static_centerline_generator/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,9 +1,9 @@ - autoware_static_centerline_generator + static_centerline_generator 0.1.0 - The autoware_static_centerline_generator package + The static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index 08bff8b80dcb7..c1cb0473da040 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -17,15 +17,15 @@ import json import uuid -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute from flask import Flask from flask import jsonify from flask import request from flask_cors import CORS import rclpy from rclpy.node import Node +from static_centerline_generator.srv import LoadMap +from static_centerline_generator.srv import PlanPath +from static_centerline_generator.srv import PlanRoute rclpy.init() node = Node("static_centerline_generator_http_server") diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 48be274aa0543..4e541b1aff527 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -20,7 +20,7 @@ #include -namespace autoware::static_centerline_generator +namespace static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node) { @@ -79,4 +79,4 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) return centerline_traj_points; } -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 1c79bccab5e79..7980ae4e8d2d7 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -22,7 +22,7 @@ #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -namespace autoware::static_centerline_generator +namespace static_centerline_generator { namespace { @@ -79,13 +79,13 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( const auto start_center_pose = utils::get_center_pose(route_handler, route_lane_ids.front()); // get ego nearest search parameters and resample interval in behavior_path_planner - const auto ego_nearest_dist_threshold = + const double ego_nearest_dist_threshold = tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_dist_threshold"); - const auto ego_nearest_yaw_threshold = + const double ego_nearest_yaw_threshold = tier4_autoware_utils::getOrDeclareParameter(node, "ego_nearest_yaw_threshold"); - const auto behavior_path_interval = + const double behavior_path_interval = tier4_autoware_utils::getOrDeclareParameter(node, "output_path_interval"); - const auto behavior_vel_interval = + const double behavior_vel_interval = tier4_autoware_utils::getOrDeclareParameter(node, "behavior_output_path_interval"); // extract path with lane id from lanelets @@ -107,7 +107,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( RCLCPP_INFO(node.get_logger(), "Converted to path and published."); // smooth trajectory and road collision avoidance - auto optimized_traj_points = optimize_trajectory(raw_path); + const auto optimized_traj_points = optimize_trajectory(raw_path); RCLCPP_INFO( node.get_logger(), "Smoothed trajectory and made it collision free with the road and published."); @@ -116,7 +116,7 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization( } std::vector OptimizationTrajectoryBasedCenterline::optimize_trajectory( - const Path & raw_path) + const Path & raw_path) const { // convert to trajectory points const auto raw_traj_points = [&]() { @@ -132,8 +132,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; - const int traj_segment_num = - static_cast(raw_traj_points.size()) / valid_optimized_traj_points_num; + const int traj_segment_num = raw_traj_points.size() / valid_optimized_traj_points_num; // NOTE: num_initial_optimization exists to make the both optimizations stable since they may use // warm start. @@ -169,16 +168,16 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra if (dist < 0.5) { const std::vector extracted_whole_optimized_traj_points{ whole_optimized_traj_points.begin(), - whole_optimized_traj_points.begin() + static_cast(std::max(j, 1UL)) - 1}; + whole_optimized_traj_points.begin() + std::max(j, 1UL) - 1}; whole_optimized_traj_points = extracted_whole_optimized_traj_points; break; } } - for (const auto & optimized_traj_point : optimized_traj_points) { - whole_optimized_traj_points.push_back(optimized_traj_point); + for (size_t j = 0; j < optimized_traj_points.size(); ++j) { + whole_optimized_traj_points.push_back(optimized_traj_points.at(j)); } } return whole_optimized_traj_points; } -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp new file mode 100644 index 0000000000000..981cf54fc9274 --- /dev/null +++ b/planning/static_centerline_generator/src/main.cpp @@ -0,0 +1,39 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "static_centerline_generator/static_centerline_generator_node.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + // initialize node + rclcpp::NodeOptions node_options; + auto node = + std::make_shared(node_options); + + // get ros parameter + const bool run_background = node->declare_parameter("run_background"); + + // process + if (!run_background) { + node->run(); + } + + // NOTE: spin node to keep showing debug path/trajectory in rviz with transient local + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 9f538b418421b..ec24f47dacc27 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,7 +14,6 @@ #include "static_centerline_generator/static_centerline_generator_node.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -22,9 +21,8 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "obstacle_avoidance_planner/node.hpp" -#include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "static_centerline_generator/msg/points_with_lane_id.hpp" #include "static_centerline_generator/type_alias.hpp" #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -56,10 +54,8 @@ #include #include -namespace autoware::static_centerline_generator +namespace static_centerline_generator { -using ::tier4_autoware_utils::Point2d; - namespace { std::vector get_lane_ids_from_route(const LaneletRoute & route) @@ -120,7 +116,7 @@ geometry_msgs::msg::Pose get_text_pose( std::array convert_hex_string_to_decimal(const std::string & hex_str_color) { - unsigned int hex_int_color = 0.0; + unsigned int hex_int_color; std::istringstream iss(hex_str_color); iss >> std::hex >> hex_int_color; @@ -213,7 +209,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( }); sub_traj_resample_interval_ = create_subscription( "/centerline_updater_helper/traj_resample_interval", rclcpp::QoS{1}, - []([[maybe_unused]] const std_msgs::msg::Float32 & msg) { + [this]([[maybe_unused]] const std_msgs::msg::Float32 & msg) { // TODO(murooka) }); @@ -246,8 +242,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( if (centerline_source_param == "optimization_trajectory_base") { optimization_trajectory_based_centerline_ = OptimizationTrajectoryBasedCenterline(*this); return CenterlineSource::OptimizationTrajectoryBase; - } - if (centerline_source_param == "bag_ego_trajectory_base") { + } else if (centerline_source_param == "bag_ego_trajectory_base") { return CenterlineSource::BagEgoTrajectoryBase; } throw std::logic_error( @@ -306,8 +301,7 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout optimization_trajectory_based_centerline_.generate_centerline_with_optimization( *this, *route_handler_ptr_, route_lane_ids); return CenterlineWithRoute{optimized_centerline, route_lane_ids}; - } - if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { + } else if (centerline_source_ == CenterlineSource::BagEgoTrajectoryBase) { const auto bag_centerline = generate_centerline_with_bag(*this); const auto start_lanelets = route_handler_ptr_->getRoadLaneletsAtPose(bag_centerline.front().pose); @@ -446,14 +440,14 @@ std::vector StaticCenterlineGeneratorNode::plan_route( mission_planner->initialize(&node, map_bin_ptr_); // plan route - auto route = mission_planner->plan(check_points); + const auto route = mission_planner->plan(check_points); return route; }(); RCLCPP_INFO(get_logger(), "Planned route."); // get lanelets - auto route_lane_ids = get_lane_ids_from_route(route); + const auto route_lane_ids = get_lane_ids_from_route(route); return route_lane_ids; } @@ -548,7 +542,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( if (!current_lanelet_points.empty()) { // register points with lane_id - autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; + static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; points_with_lane_id.lane_id = lanelet.id(); points_with_lane_id.points = current_lanelet_points; response->points_with_lane_ids.push_back(points_with_lane_id); @@ -658,7 +652,4 @@ void StaticCenterlineGeneratorNode::save_map( lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", std::filesystem::copy_options::overwrite_existing); } -} // namespace autoware::static_centerline_generator -#include -RCLCPP_COMPONENTS_REGISTER_NODE( - autoware::static_centerline_generator::StaticCenterlineGeneratorNode) +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index 6aba7d5651519..ddfd3e11036ce 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -21,7 +21,7 @@ #include #include -namespace autoware::static_centerline_generator +namespace static_centerline_generator { namespace { @@ -32,7 +32,7 @@ nav_msgs::msg::Odometry::ConstSharedPtr convert_to_odometry(const geometry_msgs: return odometry_ptr; } -lanelet::Point3d create_point3d(const double x, const double y, const double z = 19.0) +lanelet::Point3d createPoint3d(const double x, const double y, const double z = 19.0) { lanelet::Point3d point(lanelet::utils::getId()); point.setAttribute("local_x", x); @@ -65,9 +65,9 @@ geometry_msgs::msg::Pose get_center_pose( const RouteHandler & route_handler, const size_t lanelet_id) { // get middle idx of the lanelet - const auto lanelet = route_handler.getLaneletsFromId(static_cast(lanelet_id)); + const auto lanelet = route_handler.getLaneletsFromId(lanelet_id); const auto center_line = lanelet.centerline(); - const size_t middle_point_idx = std::floor(static_cast(center_line.size()) / 2.0); + const size_t middle_point_idx = std::floor(center_line.size() / 2.0); // get middle position of the lanelet geometry_msgs::msg::Point middle_pos; @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose get_center_pose( } PathWithLaneId get_path_with_lane_id( - const RouteHandler & route_handler, const lanelet::ConstLanelets & lanelets, + const RouteHandler & route_handler, const lanelet::ConstLanelets lanelets, const geometry_msgs::msg::Pose & start_pose, const double ego_nearest_dist_threshold, const double ego_nearest_yaw_threshold) { @@ -110,8 +110,7 @@ PathWithLaneId get_path_with_lane_id( constexpr double vehicle_length = 0.0; const auto drivable_lanes = behavior_path_planner::utils::generateDrivableLanes(lanelets); behavior_path_planner::utils::generateDrivableArea( - path_with_lane_id, drivable_lanes, false, false, static_cast(vehicle_length), - planner_data); + path_with_lane_id, drivable_lanes, false, false, vehicle_length, planner_data); return path_with_lane_id; } @@ -142,7 +141,7 @@ void update_centerline( const lanelet::BasicPoint2d point(traj_pos.x, traj_pos.y); const bool is_inside = lanelet::geometry::inside(lanelet_ref, point); if (is_inside) { - const auto center_point = create_point3d(traj_pos.x, traj_pos.y, traj_pos.z); + const auto center_point = createPoint3d(traj_pos.x, traj_pos.y, traj_pos.z); // set center point centerline.push_back(center_point); @@ -179,11 +178,10 @@ MarkerArray create_footprint_marker( const double b = marker_color.at(2); auto marker = tier4_autoware_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints", static_cast(idx), + "map", rclcpp::Clock().now(), "unsafe_footprints", idx, visualization_msgs::msg::Marker::LINE_STRIP, tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0), - tier4_autoware_utils::createMarkerColor( - static_cast(r), static_cast(g), static_cast(b), 0.999)); + tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -212,11 +210,10 @@ MarkerArray create_distance_text_marker( const double b = marker_color.at(2); auto marker = tier4_autoware_utils::createDefaultMarker( - "map", rclcpp::Clock().now(), "unsafe_footprints_distance", static_cast(idx), + "map", rclcpp::Clock().now(), "unsafe_footprints_distance", idx, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, tier4_autoware_utils::createMarkerScale(0.5, 0.5, 0.5), - tier4_autoware_utils::createMarkerColor( - static_cast(r), static_cast(g), static_cast(b), 0.999)); + tier4_autoware_utils::createMarkerColor(r, g, b, 0.999)); marker.pose = pose; marker.header.stamp = now; marker.lifetime = rclcpp::Duration(0, 0); @@ -231,4 +228,4 @@ MarkerArray create_distance_text_marker( return marker_array; } } // namespace utils -} // namespace autoware::static_centerline_generator +} // namespace static_centerline_generator diff --git a/planning/static_centerline_generator/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv index 3a8d0321ffb9a..7d823b4eccbf9 100644 --- a/planning/static_centerline_generator/srv/PlanPath.srv +++ b/planning/static_centerline_generator/srv/PlanPath.srv @@ -1,6 +1,6 @@ uint32 map_id int64[] route --- -autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids +static_centerline_generator/PointsWithLaneId[] points_with_lane_ids string message int64[] unconnected_lane_ids diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 0995bb9bb0424..29ee49a11e3b3 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,12 +28,11 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), - "test/data/lanelet2_map.osm", + get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" ) static_centerline_generator_node = Node( - package="autoware_static_centerline_generator", + package="static_centerline_generator", executable="main", output="screen", parameters=[ @@ -51,7 +50,7 @@ def generate_test_description(): "mission_planner.param.yaml", ), os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), + get_package_share_directory("static_centerline_generator"), "config/static_centerline_generator.param.yaml", ), os.path.join( @@ -75,15 +74,15 @@ def generate_test_description(): "config/lanelet2_map_loader.param.yaml", ), os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), + get_package_share_directory("static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), + get_package_share_directory("static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), + get_package_share_directory("static_centerline_generator"), "config/vehicle_info.param.yaml", ), ], From 9d4f3f8044eb9801b30a09c931d80865942a88f6 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 18 Apr 2024 16:20:44 +0200 Subject: [PATCH 21/33] fix(autoware_static_centerline_generator): add autoware_ prefix Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 8 +++---- .../static_centerline_generator/README.md | 4 ++-- .../bag_ego_trajectory_based_centerline.hpp | 4 ++-- ...timization_trajectory_based_centerline.hpp | 4 ++-- .../static_centerline_generator_node.hpp | 16 ++++++------- .../type_alias.hpp | 4 ++-- .../static_centerline_generator/utils.hpp | 4 ++-- .../launch/run_planning_server.launch.xml | 4 ++-- .../static_centerline_generator.launch.xml | 6 ++--- .../static_centerline_generator/package.xml | 4 ++-- .../rviz/static_centerline_generator.rviz | 8 +++---- .../scripts/app.py | 12 +++++----- .../scripts/centerline_updater_helper.py | 2 +- .../scripts/show_lanelet2_map_diff.py | 6 ++--- .../bag_ego_trajectory_based_centerline.sh | 2 +- ...ptimization_trajectory_based_centerline.sh | 2 +- .../bag_ego_trajectory_based_centerline.cpp | 4 ++-- ...timization_trajectory_based_centerline.cpp | 4 ++-- .../static_centerline_generator/src/main.cpp | 2 +- .../src/static_centerline_generator_node.cpp | 24 +++++++++---------- .../static_centerline_generator/src/utils.cpp | 4 ++-- .../srv/PlanPath.srv | 2 +- .../test_static_centerline_generator.test.py | 16 ++++++------- 23 files changed, 73 insertions(+), 73 deletions(-) diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt index 991d12097cc08..3993674c89e8c 100644 --- a/planning/static_centerline_generator/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(static_centerline_generator) +project(autoware_static_centerline_generator) find_package(autoware_cmake REQUIRED) @@ -9,7 +9,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) rosidl_generate_interfaces( - static_centerline_generator + autoware_static_centerline_generator "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" @@ -29,10 +29,10 @@ ament_auto_add_executable(main if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(main - static_centerline_generator "rosidl_typesupport_cpp") + autoware_static_centerline_generator "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp") + cpp_typesupport_target autoware_static_centerline_generator "rosidl_typesupport_cpp") target_link_libraries(main "${cpp_typesupport_target}") endif() diff --git a/planning/static_centerline_generator/README.md b/planning/static_centerline_generator/README.md index 00572b754645c..844d0af15f2a5 100644 --- a/planning/static_centerline_generator/README.md +++ b/planning/static_centerline_generator/README.md @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch static_centerline_generator run_planning_server.launch.xml vehicle_model:= +ros2 launch autoware_static_centerline_generator run_planning_server.launch.xml vehicle_model:= ``` FYI, port ID of the http server is 4010 by default. @@ -50,7 +50,7 @@ The optimized centerline can be generated from the command line interface by des - `` ```sh -ros2 launch static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml run_backgrond:=false lanelet2_input_file_path:= lanelet2_output_file_path:= start_lanelet_id:= end_lanelet_id:= vehicle_model:= ``` The default output map path containing the optimized centerline locates `/tmp/lanelet2_map.osm`. diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp index 30b2e55c36bba..912194004286d 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -20,8 +20,8 @@ #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 7e7cdea31e9f1..c20da46f1375b 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -20,7 +20,7 @@ #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { class OptimizationTrajectoryBasedCenterline { @@ -37,7 +37,7 @@ class OptimizationTrajectoryBasedCenterline rclcpp::Publisher::SharedPtr pub_raw_path_with_lane_id_{nullptr}; rclcpp::Publisher::SharedPtr pub_raw_path_{nullptr}; }; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator // clang-format off #endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT // clang-format on diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index c1d92c06a8e05..6ba3d087d1532 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -17,9 +17,9 @@ #include "rclcpp/rclcpp.hpp" #include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" -#include "static_centerline_generator/srv/load_map.hpp" -#include "static_centerline_generator/srv/plan_path.hpp" -#include "static_centerline_generator/srv/plan_route.hpp" +#include "autoware_static_centerline_generator/srv/load_map.hpp" +#include "autoware_static_centerline_generator/srv/plan_path.hpp" +#include "autoware_static_centerline_generator/srv/plan_route.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -34,11 +34,11 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { -using static_centerline_generator::srv::LoadMap; -using static_centerline_generator::srv::PlanPath; -using static_centerline_generator::srv::PlanRoute; +using autoware_static_centerline_generator::srv::LoadMap; +using autoware_static_centerline_generator::srv::PlanPath; +using autoware_static_centerline_generator::srv::PlanRoute; struct CenterlineWithRoute { @@ -115,5 +115,5 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node // vehicle info vehicle_info_util::VehicleInfo vehicle_info_; }; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 2dcb9cbbd099f..8fddef2c28842 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp @@ -26,7 +26,7 @@ #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace static_centerline_generator +namespace autoware::static_centerline_generator { using autoware_auto_mapping_msgs::msg::HADMapBin; using autoware_auto_perception_msgs::msg::PredictedObjects; @@ -41,6 +41,6 @@ using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index c8cf8f8b90590..9b755f80a6888 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -24,7 +24,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace utils { @@ -53,6 +53,6 @@ MarkerArray create_distance_text_marker( const geometry_msgs::msg::Pose & pose, const double dist, const std::array & marker_color, const rclcpp::Time & now, const size_t idx); } // namespace utils -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator #endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_generator/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml index 1493078317458..cb368ca336316 100644 --- a/planning/static_centerline_generator/launch/run_planning_server.launch.xml +++ b/planning/static_centerline_generator/launch/run_planning_server.launch.xml @@ -2,11 +2,11 @@ - + - + diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index ae71b0ae6e925..09868758f798a 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + @@ -79,11 +79,11 @@ - + - + diff --git a/planning/static_centerline_generator/package.xml b/planning/static_centerline_generator/package.xml index 6573f6e439c43..96e17595d49ee 100644 --- a/planning/static_centerline_generator/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,9 +1,9 @@ - static_centerline_generator + autoware_static_centerline_generator 0.1.0 - The static_centerline_generator package + The autoware_static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 diff --git a/planning/static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/static_centerline_generator/rviz/static_centerline_generator.rviz index 62b4c9b75ec87..002af580c2f00 100644 --- a/planning/static_centerline_generator/rviz/static_centerline_generator.rviz +++ b/planning/static_centerline_generator/rviz/static_centerline_generator.rviz @@ -130,7 +130,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/input_centerline + Value: /autoware_static_centerline_generator/input_centerline Value: true View Drivable Area: Alpha: 0.9990000128746033 @@ -179,7 +179,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/output_centerline + Value: /autoware_static_centerline_generator/output_centerline Value: true View Footprint: Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/debug/raw_centerline + Value: /autoware_static_centerline_generator/debug/raw_centerline Value: false View Drivable Area: Alpha: 0.9990000128746033 @@ -268,7 +268,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_generator/debug/unsafe_footprints + Value: /autoware_static_centerline_generator/debug/unsafe_footprints Value: true Enabled: false Name: debug diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index c1cb0473da040..e13ba87953ef9 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -23,9 +23,9 @@ from flask_cors import CORS import rclpy from rclpy.node import Node -from static_centerline_generator.srv import LoadMap -from static_centerline_generator.srv import PlanPath -from static_centerline_generator.srv import PlanRoute +from autoware_static_centerline_generator.srv import LoadMap +from autoware_static_centerline_generator.srv import PlanPath +from autoware_static_centerline_generator.srv import PlanRoute rclpy.init() node = Node("static_centerline_generator_http_server") @@ -51,7 +51,7 @@ def get_map(): map_id = map_uuid # create client - cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") + cli = create_client(LoadMap, "/planning/autoware_static_centerline_generator/load_map") # request map loading req = LoadMap.Request(map=data["map"]) @@ -85,7 +85,7 @@ def post_planned_route(): print("map_id is not correct.") # create client - cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") + cli = create_client(PlanRoute, "/planning/autoware_static_centerline_generator/plan_route") # request route planning req = PlanRoute.Request( @@ -123,7 +123,7 @@ def post_planned_path(): print("map_id is not correct.") # create client - cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") + cli = create_client(PlanPath, "/planning/autoware_static_centerline_generator/plan_path") # request path planning route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] diff --git a/planning/static_centerline_generator/scripts/centerline_updater_helper.py b/planning/static_centerline_generator/scripts/centerline_updater_helper.py index fec3d21d20bec..cec4e5b457c7f 100755 --- a/planning/static_centerline_generator/scripts/centerline_updater_helper.py +++ b/planning/static_centerline_generator/scripts/centerline_updater_helper.py @@ -155,7 +155,7 @@ def __init__(self): transient_local_qos = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) self.sub_whole_centerline = self.create_subscription( Trajectory, - "/static_centerline_generator/output_whole_centerline", + "/autoware_static_centerline_generator/output_whole_centerline", self.onWholeCenterline, transient_local_qos, ) diff --git a/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py index 912226511f1d9..00d06ca2213d1 100755 --- a/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py +++ b/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py @@ -98,8 +98,8 @@ def remove_diff_to_ignore(osm_root): ) args = parser.parse_args() - original_osm_file_name = "/tmp/static_centerline_generator/input/lanelet2_map.osm" - modified_osm_file_name = "/tmp/static_centerline_generator/output/lanelet2_map.osm" + original_osm_file_name = "/tmp/autoware_static_centerline_generator/input/lanelet2_map.osm" + modified_osm_file_name = "/tmp/autoware_static_centerline_generator/output/lanelet2_map.osm" # load LL2 maps original_osm_tree = ET.parse(original_osm_file_name) @@ -118,7 +118,7 @@ def remove_diff_to_ignore(osm_root): remove_diff_to_ignore(modified_osm_root) # write LL2 maps - output_dir_path = "/tmp/static_centerline_generator/show_lanelet2_map_diff/" + output_dir_path = "/tmp/autoware_static_centerline_generator/show_lanelet2_map_diff/" os.makedirs(output_dir_path + "original/", exist_ok=True) os.makedirs(output_dir_path + "modified/", exist_ok=True) diff --git a/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh index 30ed667dd3732..e7f86062a9d20 100755 --- a/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ b/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -1,3 +1,3 @@ #!/bin/bash -ros2 launch static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="bag_ego_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" bag_filename:="$(ros2 pkg prefix autoware_static_centerline_generator --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus diff --git a/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh index 809bbb46a179e..488091989d1fa 100755 --- a/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh +++ b/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh @@ -1,3 +1,3 @@ #!/bin/bash -ros2 launch static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus +ros2 launch autoware_static_centerline_generator static_centerline_generator.launch.xml centerline_source:="optimization_trajectory_base" run_background:=false lanelet2_input_file_path:="$HOME/autoware_map/sample_map/lanelet2_map.osm" start_lanelet_id:=125 end_lanelet_id:=132 vehicle_model:=lexus diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 4e541b1aff527..48be274aa0543 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -20,7 +20,7 @@ #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node) { @@ -79,4 +79,4 @@ std::vector generate_centerline_with_bag(rclcpp::Node & node) return centerline_traj_points; } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 7980ae4e8d2d7..3c22740cd10d5 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -22,7 +22,7 @@ #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -180,4 +180,4 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra return whole_optimized_traj_points; } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 981cf54fc9274..ea68ef81e7b76 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; auto node = - std::make_shared(node_options); + std::make_shared(node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index ec24f47dacc27..6d8b28e9a1326 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -22,7 +22,7 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "static_centerline_generator/msg/points_with_lane_id.hpp" +#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "static_centerline_generator/type_alias.hpp" #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -54,7 +54,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -168,7 +168,7 @@ std::vector resample_trajectory_points( StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const rclcpp::NodeOptions & node_options) -: Node("static_centerline_generator", node_options) +: Node("autoware_static_centerline_generator", node_options) { // publishers pub_map_bin_ = @@ -216,19 +216,19 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_load_map_ = create_service( - "/planning/static_centerline_generator/load_map", + "/planning/autoware_static_centerline_generator/load_map", std::bind( &StaticCenterlineGeneratorNode::on_load_map, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_route_ = create_service( - "/planning/static_centerline_generator/plan_route", + "/planning/autoware_static_centerline_generator/plan_route", std::bind( &StaticCenterlineGeneratorNode::on_plan_route, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); srv_plan_path_ = create_service( - "/planning/static_centerline_generator/plan_path", + "/planning/autoware_static_centerline_generator/plan_path", std::bind( &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, std::placeholders::_2), @@ -246,7 +246,7 @@ StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( return CenterlineSource::BagEgoTrajectoryBase; } throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); + "The centerline source is not supported in autoware_static_centerline_generator."); }(); } @@ -319,7 +319,7 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( - "The centerline source is not supported in static_centerline_generator."); + "The centerline source is not supported in autoware_static_centerline_generator."); }(); // resample @@ -339,7 +339,7 @@ CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_rout void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // copy the input LL2 map to the temporary file for debugging - const std::string debug_input_file_dir{"/tmp/static_centerline_generator/input/"}; + const std::string debug_input_file_dir{"/tmp/autoware_static_centerline_generator/input/"}; std::filesystem::create_directories(debug_input_file_dir); std::filesystem::copy( lanelet2_input_file_path, debug_input_file_dir + "lanelet2_map.osm", @@ -542,7 +542,7 @@ void StaticCenterlineGeneratorNode::on_plan_path( if (!current_lanelet_points.empty()) { // register points with lane_id - static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; + autoware_static_centerline_generator::msg::PointsWithLaneId points_with_lane_id; points_with_lane_id.lane_id = lanelet.id(); points_with_lane_id.points = current_lanelet_points; response->points_with_lane_ids.push_back(points_with_lane_id); @@ -646,10 +646,10 @@ void StaticCenterlineGeneratorNode::save_map( RCLCPP_INFO(get_logger(), "Saved map."); // copy the output LL2 map to the temporary file for debugging - const std::string debug_output_file_dir{"/tmp/static_centerline_generator/output/"}; + const std::string debug_output_file_dir{"/tmp/autoware_static_centerline_generator/output/"}; std::filesystem::create_directories(debug_output_file_dir); std::filesystem::copy( lanelet2_output_file_path, debug_output_file_dir + "lanelet2_map.osm", std::filesystem::copy_options::overwrite_existing); } -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index ddfd3e11036ce..c3678a4b2349b 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -21,7 +21,7 @@ #include #include -namespace static_centerline_generator +namespace autoware::static_centerline_generator { namespace { @@ -228,4 +228,4 @@ MarkerArray create_distance_text_marker( return marker_array; } } // namespace utils -} // namespace static_centerline_generator +} // namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv index 7d823b4eccbf9..3a8d0321ffb9a 100644 --- a/planning/static_centerline_generator/srv/PlanPath.srv +++ b/planning/static_centerline_generator/srv/PlanPath.srv @@ -1,6 +1,6 @@ uint32 map_id int64[] route --- -static_centerline_generator/PointsWithLaneId[] points_with_lane_ids +autoware_static_centerline_generator/PointsWithLaneId[] points_with_lane_ids string message int64[] unconnected_lane_ids diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 29ee49a11e3b3..bde33117d8cc7 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,11 +28,11 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" + get_package_share_directory("autoware_static_centerline_generator"), "test/data/lanelet2_map.osm" ) static_centerline_generator_node = Node( - package="static_centerline_generator", + package="autoware_static_centerline_generator", executable="main", output="screen", parameters=[ @@ -50,8 +50,8 @@ def generate_test_description(): "mission_planner.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), - "config/static_centerline_generator.param.yaml", + get_package_share_directory("autoware_static_centerline_generator"), + "config/autoware_static_centerline_generator.param.yaml", ), os.path.join( get_package_share_directory("behavior_path_planner"), @@ -74,15 +74,15 @@ def generate_test_description(): "config/lanelet2_map_loader.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_generator"), + get_package_share_directory("autoware_static_centerline_generator"), "config/vehicle_info.param.yaml", ), ], @@ -94,7 +94,7 @@ def generate_test_description(): LaunchDescription( [ static_centerline_generator_node, - # Start test after 1s - gives time for the static_centerline_generator to finish initialization + # Start test after 1s - gives time for the autoware_static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), From f65f6c790325c325bd1adc74824289738ff1dc2f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 18 Apr 2024 14:22:54 +0000 Subject: [PATCH 22/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../static_centerline_generator_node.hpp | 4 ++-- planning/static_centerline_generator/scripts/app.py | 6 +++--- planning/static_centerline_generator/src/main.cpp | 4 ++-- .../src/static_centerline_generator_node.cpp | 2 +- .../test/test_static_centerline_generator.test.py | 3 ++- 5 files changed, 10 insertions(+), 9 deletions(-) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 6ba3d087d1532..aa0d133f75e7b 100644 --- a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -15,11 +15,11 @@ #ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" +#include "rclcpp/rclcpp.hpp" +#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" #include "static_centerline_generator/type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/static_centerline_generator/scripts/app.py index e13ba87953ef9..f37737a0de4e4 100755 --- a/planning/static_centerline_generator/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -17,15 +17,15 @@ import json import uuid +from autoware_static_centerline_generator.srv import LoadMap +from autoware_static_centerline_generator.srv import PlanPath +from autoware_static_centerline_generator.srv import PlanRoute from flask import Flask from flask import jsonify from flask import request from flask_cors import CORS import rclpy from rclpy.node import Node -from autoware_static_centerline_generator.srv import LoadMap -from autoware_static_centerline_generator.srv import PlanPath -from autoware_static_centerline_generator.srv import PlanRoute rclpy.init() node = Node("static_centerline_generator_http_server") diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index ea68ef81e7b76..5d71cc2faf6bd 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -20,8 +20,8 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; - auto node = - std::make_shared(node_options); + auto node = std::make_shared( + node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 6d8b28e9a1326..fe322f4f3c159 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -14,6 +14,7 @@ #include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -22,7 +23,6 @@ #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "static_centerline_generator/type_alias.hpp" #include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index bde33117d8cc7..c1708b50b9168 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -28,7 +28,8 @@ @pytest.mark.launch_test def generate_test_description(): lanelet2_map_path = os.path.join( - get_package_share_directory("autoware_static_centerline_generator"), "test/data/lanelet2_map.osm" + get_package_share_directory("autoware_static_centerline_generator"), + "test/data/lanelet2_map.osm", ) static_centerline_generator_node = Node( From ba9e0ba0b2d53768184c9fe61e165e8cf01ed684 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 18 Apr 2024 16:30:42 +0200 Subject: [PATCH 23/33] fix(autoware_static_centerline_generator): fix filenames Signed-off-by: Esteve Fernandez --- .../launch/static_centerline_generator.launch.xml | 4 ++-- .../test/test_static_centerline_generator.test.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index 09868758f798a..5c6ffca3637fb 100644 --- a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -79,11 +79,11 @@ - + - + diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py index c1708b50b9168..3011abccd48ca 100644 --- a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py +++ b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py @@ -52,7 +52,7 @@ def generate_test_description(): ), os.path.join( get_package_share_directory("autoware_static_centerline_generator"), - "config/autoware_static_centerline_generator.param.yaml", + "config/static_centerline_generator.param.yaml", ), os.path.join( get_package_share_directory("behavior_path_planner"), From e0f836392e63ab33e508a69af141f906a830d326 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Thu, 18 Apr 2024 16:32:06 +0200 Subject: [PATCH 24/33] fix(autoware_static_centerline_generator): fix namespaces Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 5d71cc2faf6bd..8e6de07a1a641 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -20,7 +20,7 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; - auto node = std::make_shared( + auto node = std::make_shared( node_options); // get ros parameter From 3bf323ff79d1446cd1ac0f1d163986106afd520e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 18 Apr 2024 14:35:24 +0000 Subject: [PATCH 25/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- planning/static_centerline_generator/src/main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 8e6de07a1a641..7a5e215e2f975 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -20,8 +20,9 @@ int main(int argc, char * argv[]) // initialize node rclcpp::NodeOptions node_options; - auto node = std::make_shared( - node_options); + auto node = + std::make_shared( + node_options); // get ros parameter const bool run_background = node->declare_parameter("run_background"); From f88911daa400bf4ff4a6ce57ec5b3a176dd99780 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 May 2024 16:17:29 +0200 Subject: [PATCH 26/33] fix: added prefix to missing strings Signed-off-by: Esteve Fernandez --- .github/CODEOWNERS | 2 +- planning/.pages | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index dc745760cb7d0..4c01604c51ee4 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -156,6 +156,7 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai +planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp @@ -208,7 +209,6 @@ planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp planning/sampling_based_planner/path_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/sampler_common/** maxime.clement@tier4.jp planning/scenario_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/static_centerline_generator/** kosuke.takeuchi@tier4.jp takayuki.murooka@tier4.jp planning/surround_obstacle_checker/** satoshi.ota@tier4.jp sensing/gnss_poser/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp sensing/image_diagnostics/** dai.nguyen@tier4.jp yoshi.ri@tier4.jp diff --git a/planning/.pages b/planning/.pages index 942a5a0b32158..645a32b4b05fa 100644 --- a/planning/.pages +++ b/planning/.pages @@ -67,7 +67,7 @@ nav: - 'About Motion Velocity Smoother': planning/motion_velocity_smoother - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja - 'Scenario Selector': planning/scenario_selector - - 'Static Centerline Generator': planning/static_centerline_generator + - 'Static Centerline Generator': planning/autoware_static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - 'External Velocity Limit Selector': planning/external_velocity_limit_selector From f31a617adb25778b11383e937dd695ba4e47fcfb Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 May 2024 16:23:24 +0200 Subject: [PATCH 27/33] refactor(autoware_static_centerline_generator): move header files to src Signed-off-by: Esteve Fernandez --- .../centerline_source/bag_ego_trajectory_based_centerline.hpp | 0 .../optimization_trajectory_based_centerline.hpp | 0 .../static_centerline_generator_node.hpp | 0 .../{include/static_centerline_generator => src}/type_alias.hpp | 0 .../{include/static_centerline_generator => src}/utils.hpp | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename planning/static_centerline_generator/{include/static_centerline_generator => src}/centerline_source/bag_ego_trajectory_based_centerline.hpp (100%) rename planning/static_centerline_generator/{include/static_centerline_generator => src}/centerline_source/optimization_trajectory_based_centerline.hpp (100%) rename planning/static_centerline_generator/{include/static_centerline_generator => src}/static_centerline_generator_node.hpp (100%) rename planning/static_centerline_generator/{include/static_centerline_generator => src}/type_alias.hpp (100%) rename planning/static_centerline_generator/{include/static_centerline_generator => src}/utils.hpp (100%) diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp similarity index 100% rename from planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp rename to planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp similarity index 100% rename from planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp rename to planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp b/planning/static_centerline_generator/src/static_centerline_generator_node.hpp similarity index 100% rename from planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp rename to planning/static_centerline_generator/src/static_centerline_generator_node.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp b/planning/static_centerline_generator/src/type_alias.hpp similarity index 100% rename from planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp rename to planning/static_centerline_generator/src/type_alias.hpp diff --git a/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp b/planning/static_centerline_generator/src/utils.hpp similarity index 100% rename from planning/static_centerline_generator/include/static_centerline_generator/utils.hpp rename to planning/static_centerline_generator/src/utils.hpp From 550640cf933b812242b05183939409496642c9f0 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 May 2024 16:25:54 +0200 Subject: [PATCH 28/33] refactor(autoware_static_centerline_generator): fix include paths Signed-off-by: Esteve Fernandez --- .../bag_ego_trajectory_based_centerline.cpp | 4 ++-- .../bag_ego_trajectory_based_centerline.hpp | 2 +- .../optimization_trajectory_based_centerline.cpp | 6 +++--- .../optimization_trajectory_based_centerline.hpp | 2 +- planning/static_centerline_generator/src/main.cpp | 2 +- .../src/static_centerline_generator_node.cpp | 8 ++++---- .../src/static_centerline_generator_node.hpp | 4 ++-- planning/static_centerline_generator/src/utils.cpp | 2 +- planning/static_centerline_generator/src/utils.hpp | 2 +- 9 files changed, 16 insertions(+), 16 deletions(-) diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 48be274aa0543..0ccceaf71d216 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "rclcpp/serialization.hpp" #include "rosbag2_cpp/reader.hpp" -#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator_node.hpp" #include diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp index 912194004286d..339b24d673798 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -16,7 +16,7 @@ #define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "type_alias.hpp" #include diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 3c22740cd10d5..5a522c143cd69 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" +#include "centerline_source/optimization_trajectory_based_centerline.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" #include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" -#include "static_centerline_generator/static_centerline_generator_node.hpp" -#include "static_centerline_generator/utils.hpp" +#include "static_centerline_generator_node.hpp" +#include "utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" namespace autoware::static_centerline_generator diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp index c20da46f1375b..6872a5878f52f 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp @@ -16,7 +16,7 @@ #define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "type_alias.hpp" #include diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/static_centerline_generator/src/main.cpp index 7a5e215e2f975..9f52f271cd5e5 100644 --- a/planning/static_centerline_generator/src/main.cpp +++ b/planning/static_centerline_generator/src/main.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator_node.hpp" int main(int argc, char * argv[]) { diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index fe322f4f3c159..ab2ad985138a9 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator_node.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" @@ -22,9 +22,9 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "static_centerline_generator/type_alias.hpp" -#include "static_centerline_generator/utils.hpp" +#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "type_alias.hpp" +#include "utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/static_centerline_generator/src/static_centerline_generator_node.hpp index aa0d133f75e7b..cab6db43d8fa6 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.hpp @@ -19,8 +19,8 @@ #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" #include "rclcpp/rclcpp.hpp" -#include "static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" #include diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp index c3678a4b2349b..252de0a8dde82 100644 --- a/planning/static_centerline_generator/src/utils.cpp +++ b/planning/static_centerline_generator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_generator/utils.hpp" +#include "utils.hpp" #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" diff --git a/planning/static_centerline_generator/src/utils.hpp b/planning/static_centerline_generator/src/utils.hpp index 9b755f80a6888..37e3583de9977 100644 --- a/planning/static_centerline_generator/src/utils.hpp +++ b/planning/static_centerline_generator/src/utils.hpp @@ -16,7 +16,7 @@ #define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ #include "route_handler/route_handler.hpp" -#include "static_centerline_generator/type_alias.hpp" +#include "type_alias.hpp" #include From f4511ee419961f59fd505a8f399191fef3dac7db Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 13 May 2024 14:28:05 +0000 Subject: [PATCH 29/33] style(pre-commit): autofix Signed-off-by: Esteve Fernandez --- .../bag_ego_trajectory_based_centerline.hpp | 6 +++--- .../optimization_trajectory_based_centerline.cpp | 2 +- .../optimization_trajectory_based_centerline.hpp | 6 +++--- .../src/static_centerline_generator_node.cpp | 6 +++--- .../src/static_centerline_generator_node.hpp | 8 ++++---- planning/static_centerline_generator/src/type_alias.hpp | 6 +++--- planning/static_centerline_generator/src/utils.hpp | 6 +++--- 7 files changed, 20 insertions(+), 20 deletions(-) diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp index 339b24d673798..2275f88184c6f 100644 --- a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ -#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#ifndef CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#define CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" @@ -24,4 +24,4 @@ namespace autoware::static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node); } // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#endif // CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 5a522c143cd69..7ac19837e5729 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -19,8 +19,8 @@ #include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" #include "static_centerline_generator_node.hpp" -#include "utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" +#include "utils.hpp" namespace autoware::static_centerline_generator { diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp index 6872a5878f52f..88def6fa7bd4c 100644 --- a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#ifndef CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#define CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" @@ -39,5 +39,5 @@ class OptimizationTrajectoryBasedCenterline }; } // namespace autoware::static_centerline_generator // clang-format off -#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#endif // CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT // clang-format on diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp index ab2ad985138a9..2765894cc4b0b 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp @@ -15,6 +15,7 @@ #include "static_centerline_generator_node.hpp" #include "autoware_static_centerline_generator/msg/points_with_lane_id.hpp" +#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" #include "lanelet2_extension/utility/utilities.hpp" @@ -22,11 +23,10 @@ #include "map_projection_loader/load_info_from_lanelet2_map.hpp" #include "motion_utils/resample/resample.hpp" #include "motion_utils/trajectory/conversion.hpp" -#include "centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "type_alias.hpp" -#include "utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" +#include "type_alias.hpp" +#include "utils.hpp" #include #include diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/static_centerline_generator/src/static_centerline_generator_node.hpp index cab6db43d8fa6..9baa1c3fbb892 100644 --- a/planning/static_centerline_generator/src/static_centerline_generator_node.hpp +++ b/planning/static_centerline_generator/src/static_centerline_generator_node.hpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ -#define STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#define STATIC_CENTERLINE_GENERATOR_NODE_HPP_ #include "autoware_static_centerline_generator/srv/load_map.hpp" #include "autoware_static_centerline_generator/srv/plan_path.hpp" #include "autoware_static_centerline_generator/srv/plan_route.hpp" -#include "rclcpp/rclcpp.hpp" #include "centerline_source/optimization_trajectory_based_centerline.hpp" +#include "rclcpp/rclcpp.hpp" #include "type_alias.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -116,4 +116,4 @@ class StaticCenterlineGeneratorNode : public rclcpp::Node vehicle_info_util::VehicleInfo vehicle_info_; }; } // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_generator/src/type_alias.hpp b/planning/static_centerline_generator/src/type_alias.hpp index 8fddef2c28842..fb54804db105d 100644 --- a/planning/static_centerline_generator/src/type_alias.hpp +++ b/planning/static_centerline_generator/src/type_alias.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ -#define STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#ifndef TYPE_ALIAS_HPP_ +#define TYPE_ALIAS_HPP_ #include "route_handler/route_handler.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -43,4 +43,4 @@ using tier4_autoware_utils::Point2d; using visualization_msgs::msg::MarkerArray; } // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ +#endif // TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_generator/src/utils.hpp b/planning/static_centerline_generator/src/utils.hpp index 37e3583de9977..6c6257bee59a3 100644 --- a/planning/static_centerline_generator/src/utils.hpp +++ b/planning/static_centerline_generator/src/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ -#define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ +#ifndef UTILS_HPP_ +#define UTILS_HPP_ #include "route_handler/route_handler.hpp" #include "type_alias.hpp" @@ -55,4 +55,4 @@ MarkerArray create_distance_text_marker( } // namespace utils } // namespace autoware::static_centerline_generator -#endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ +#endif // UTILS_HPP_ From 0d036221bb4b7ab27dfd2d6ea5d96f4be9dce1fb Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Mon, 13 May 2024 16:30:17 +0200 Subject: [PATCH 30/33] refactor(autoware_static_centerline_generator): rename base folder Signed-off-by: Esteve Fernandez --- .../CMakeLists.txt | 0 .../README.md | 0 .../config/common.param.yaml | 0 .../config/nearest_search.param.yaml | 0 .../config/static_centerline_generator.param.yaml | 0 .../config/vehicle_info.param.yaml | 0 .../launch/run_planning_server.launch.xml | 0 .../launch/static_centerline_generator.launch.xml | 0 .../media/rviz.png | Bin .../media/unsafe_footprints.png | Bin .../msg/PointsWithLaneId.msg | 0 .../package.xml | 0 .../rviz/static_centerline_generator.rviz | 0 .../scripts/app.py | 0 .../scripts/centerline_updater_helper.py | 0 .../scripts/show_lanelet2_map_diff.py | 0 .../tmp/bag_ego_trajectory_based_centerline.sh | 0 .../tmp/optimization_trajectory_based_centerline.sh | 0 .../bag_ego_trajectory_based_centerline.cpp | 0 .../bag_ego_trajectory_based_centerline.hpp | 0 .../optimization_trajectory_based_centerline.cpp | 0 .../optimization_trajectory_based_centerline.hpp | 0 .../src/main.cpp | 0 .../src/static_centerline_generator_node.cpp | 0 .../src/static_centerline_generator_node.hpp | 0 .../src/type_alias.hpp | 0 .../src/utils.cpp | 0 .../src/utils.hpp | 0 .../srv/LoadMap.srv | 0 .../srv/PlanPath.srv | 0 .../srv/PlanRoute.srv | 0 .../test/data/bag_ego_trajectory.db3 | Bin .../test/data/lanelet2_map.osm | 0 .../test/test_static_centerline_generator.test.py | 0 34 files changed, 0 insertions(+), 0 deletions(-) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/CMakeLists.txt (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/README.md (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/config/common.param.yaml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/config/nearest_search.param.yaml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/config/static_centerline_generator.param.yaml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/config/vehicle_info.param.yaml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/launch/run_planning_server.launch.xml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/launch/static_centerline_generator.launch.xml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/media/rviz.png (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/media/unsafe_footprints.png (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/msg/PointsWithLaneId.msg (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/package.xml (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/rviz/static_centerline_generator.rviz (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/scripts/app.py (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/scripts/centerline_updater_helper.py (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/scripts/show_lanelet2_map_diff.py (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/scripts/tmp/bag_ego_trajectory_based_centerline.sh (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/scripts/tmp/optimization_trajectory_based_centerline.sh (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/centerline_source/bag_ego_trajectory_based_centerline.cpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/centerline_source/bag_ego_trajectory_based_centerline.hpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/centerline_source/optimization_trajectory_based_centerline.cpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/centerline_source/optimization_trajectory_based_centerline.hpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/main.cpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/static_centerline_generator_node.cpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/static_centerline_generator_node.hpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/type_alias.hpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/utils.cpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/src/utils.hpp (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/srv/LoadMap.srv (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/srv/PlanPath.srv (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/srv/PlanRoute.srv (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/test/data/bag_ego_trajectory.db3 (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/test/data/lanelet2_map.osm (100%) rename planning/{static_centerline_generator => autoware_static_centerline_generator}/test/test_static_centerline_generator.test.py (100%) diff --git a/planning/static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt similarity index 100% rename from planning/static_centerline_generator/CMakeLists.txt rename to planning/autoware_static_centerline_generator/CMakeLists.txt diff --git a/planning/static_centerline_generator/README.md b/planning/autoware_static_centerline_generator/README.md similarity index 100% rename from planning/static_centerline_generator/README.md rename to planning/autoware_static_centerline_generator/README.md diff --git a/planning/static_centerline_generator/config/common.param.yaml b/planning/autoware_static_centerline_generator/config/common.param.yaml similarity index 100% rename from planning/static_centerline_generator/config/common.param.yaml rename to planning/autoware_static_centerline_generator/config/common.param.yaml diff --git a/planning/static_centerline_generator/config/nearest_search.param.yaml b/planning/autoware_static_centerline_generator/config/nearest_search.param.yaml similarity index 100% rename from planning/static_centerline_generator/config/nearest_search.param.yaml rename to planning/autoware_static_centerline_generator/config/nearest_search.param.yaml diff --git a/planning/static_centerline_generator/config/static_centerline_generator.param.yaml b/planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml similarity index 100% rename from planning/static_centerline_generator/config/static_centerline_generator.param.yaml rename to planning/autoware_static_centerline_generator/config/static_centerline_generator.param.yaml diff --git a/planning/static_centerline_generator/config/vehicle_info.param.yaml b/planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml similarity index 100% rename from planning/static_centerline_generator/config/vehicle_info.param.yaml rename to planning/autoware_static_centerline_generator/config/vehicle_info.param.yaml diff --git a/planning/static_centerline_generator/launch/run_planning_server.launch.xml b/planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml similarity index 100% rename from planning/static_centerline_generator/launch/run_planning_server.launch.xml rename to planning/autoware_static_centerline_generator/launch/run_planning_server.launch.xml diff --git a/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml similarity index 100% rename from planning/static_centerline_generator/launch/static_centerline_generator.launch.xml rename to planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml diff --git a/planning/static_centerline_generator/media/rviz.png b/planning/autoware_static_centerline_generator/media/rviz.png similarity index 100% rename from planning/static_centerline_generator/media/rviz.png rename to planning/autoware_static_centerline_generator/media/rviz.png diff --git a/planning/static_centerline_generator/media/unsafe_footprints.png b/planning/autoware_static_centerline_generator/media/unsafe_footprints.png similarity index 100% rename from planning/static_centerline_generator/media/unsafe_footprints.png rename to planning/autoware_static_centerline_generator/media/unsafe_footprints.png diff --git a/planning/static_centerline_generator/msg/PointsWithLaneId.msg b/planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg similarity index 100% rename from planning/static_centerline_generator/msg/PointsWithLaneId.msg rename to planning/autoware_static_centerline_generator/msg/PointsWithLaneId.msg diff --git a/planning/static_centerline_generator/package.xml b/planning/autoware_static_centerline_generator/package.xml similarity index 100% rename from planning/static_centerline_generator/package.xml rename to planning/autoware_static_centerline_generator/package.xml diff --git a/planning/static_centerline_generator/rviz/static_centerline_generator.rviz b/planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz similarity index 100% rename from planning/static_centerline_generator/rviz/static_centerline_generator.rviz rename to planning/autoware_static_centerline_generator/rviz/static_centerline_generator.rviz diff --git a/planning/static_centerline_generator/scripts/app.py b/planning/autoware_static_centerline_generator/scripts/app.py similarity index 100% rename from planning/static_centerline_generator/scripts/app.py rename to planning/autoware_static_centerline_generator/scripts/app.py diff --git a/planning/static_centerline_generator/scripts/centerline_updater_helper.py b/planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py similarity index 100% rename from planning/static_centerline_generator/scripts/centerline_updater_helper.py rename to planning/autoware_static_centerline_generator/scripts/centerline_updater_helper.py diff --git a/planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py b/planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py similarity index 100% rename from planning/static_centerline_generator/scripts/show_lanelet2_map_diff.py rename to planning/autoware_static_centerline_generator/scripts/show_lanelet2_map_diff.py diff --git a/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh similarity index 100% rename from planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh rename to planning/autoware_static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh diff --git a/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh similarity index 100% rename from planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh rename to planning/autoware_static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp similarity index 100% rename from planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp rename to planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp diff --git a/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp similarity index 100% rename from planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp rename to planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.hpp diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp similarity index 100% rename from planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp rename to planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp diff --git a/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp similarity index 100% rename from planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp rename to planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp diff --git a/planning/static_centerline_generator/src/main.cpp b/planning/autoware_static_centerline_generator/src/main.cpp similarity index 100% rename from planning/static_centerline_generator/src/main.cpp rename to planning/autoware_static_centerline_generator/src/main.cpp diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp similarity index 100% rename from planning/static_centerline_generator/src/static_centerline_generator_node.cpp rename to planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp diff --git a/planning/static_centerline_generator/src/static_centerline_generator_node.hpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp similarity index 100% rename from planning/static_centerline_generator/src/static_centerline_generator_node.hpp rename to planning/autoware_static_centerline_generator/src/static_centerline_generator_node.hpp diff --git a/planning/static_centerline_generator/src/type_alias.hpp b/planning/autoware_static_centerline_generator/src/type_alias.hpp similarity index 100% rename from planning/static_centerline_generator/src/type_alias.hpp rename to planning/autoware_static_centerline_generator/src/type_alias.hpp diff --git a/planning/static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp similarity index 100% rename from planning/static_centerline_generator/src/utils.cpp rename to planning/autoware_static_centerline_generator/src/utils.cpp diff --git a/planning/static_centerline_generator/src/utils.hpp b/planning/autoware_static_centerline_generator/src/utils.hpp similarity index 100% rename from planning/static_centerline_generator/src/utils.hpp rename to planning/autoware_static_centerline_generator/src/utils.hpp diff --git a/planning/static_centerline_generator/srv/LoadMap.srv b/planning/autoware_static_centerline_generator/srv/LoadMap.srv similarity index 100% rename from planning/static_centerline_generator/srv/LoadMap.srv rename to planning/autoware_static_centerline_generator/srv/LoadMap.srv diff --git a/planning/static_centerline_generator/srv/PlanPath.srv b/planning/autoware_static_centerline_generator/srv/PlanPath.srv similarity index 100% rename from planning/static_centerline_generator/srv/PlanPath.srv rename to planning/autoware_static_centerline_generator/srv/PlanPath.srv diff --git a/planning/static_centerline_generator/srv/PlanRoute.srv b/planning/autoware_static_centerline_generator/srv/PlanRoute.srv similarity index 100% rename from planning/static_centerline_generator/srv/PlanRoute.srv rename to planning/autoware_static_centerline_generator/srv/PlanRoute.srv diff --git a/planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 b/planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 similarity index 100% rename from planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 rename to planning/autoware_static_centerline_generator/test/data/bag_ego_trajectory.db3 diff --git a/planning/static_centerline_generator/test/data/lanelet2_map.osm b/planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm similarity index 100% rename from planning/static_centerline_generator/test/data/lanelet2_map.osm rename to planning/autoware_static_centerline_generator/test/data/lanelet2_map.osm diff --git a/planning/static_centerline_generator/test/test_static_centerline_generator.test.py b/planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py similarity index 100% rename from planning/static_centerline_generator/test/test_static_centerline_generator.test.py rename to planning/autoware_static_centerline_generator/test/test_static_centerline_generator.test.py From cc745f1fc4e4c56a05d8f113a0d2b3d2844dfae9 Mon Sep 17 00:00:00 2001 From: Esteve Fernandez <33620+esteve@users.noreply.github.com> Date: Wed, 15 May 2024 11:31:27 +0200 Subject: [PATCH 31/33] Update planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: M. Fatih Cırıt Signed-off-by: Esteve Fernandez --- .../launch/static_centerline_generator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml index 5c6ffca3637fb..2a2fbe4340fa8 100644 --- a/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml +++ b/planning/autoware_static_centerline_generator/launch/static_centerline_generator.launch.xml @@ -55,7 +55,7 @@ - + From e1902a4db19bd4c5bfdeb88f914e669fce3f5a2c Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 15 May 2024 12:25:48 +0200 Subject: [PATCH 32/33] build(autoware_static_centerline_generator): fix include in CMake Signed-off-by: Esteve Fernandez --- planning/autoware_static_centerline_generator/CMakeLists.txt | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/planning/autoware_static_centerline_generator/CMakeLists.txt b/planning/autoware_static_centerline_generator/CMakeLists.txt index 3993674c89e8c..08a97c9010008 100644 --- a/planning/autoware_static_centerline_generator/CMakeLists.txt +++ b/planning/autoware_static_centerline_generator/CMakeLists.txt @@ -43,6 +43,10 @@ ament_auto_package( rviz ) +target_include_directories(main PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src +) + if(BUILD_TESTING) add_launch_test( test/test_static_centerline_generator.test.py From de05c293abff0bfb71397001f2da5f4c198883fe Mon Sep 17 00:00:00 2001 From: Esteve Fernandez Date: Wed, 15 May 2024 12:27:55 +0200 Subject: [PATCH 33/33] build(autoware_static_centerline_generator): fix missing includes Signed-off-by: Esteve Fernandez --- .../centerline_source/bag_ego_trajectory_based_centerline.cpp | 3 +++ .../optimization_trajectory_based_centerline.cpp | 2 ++ .../src/static_centerline_generator_node.cpp | 1 + planning/autoware_static_centerline_generator/src/utils.cpp | 3 +++ 4 files changed, 9 insertions(+) diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 0ccceaf71d216..447773d7a6535 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -20,6 +20,9 @@ #include +#include +#include + namespace autoware::static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node) diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 7ac19837e5729..2825381937674 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -22,6 +22,8 @@ #include "tier4_autoware_utils/ros/parameter.hpp" #include "utils.hpp" +#include + namespace autoware::static_centerline_generator { namespace diff --git a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp index 2765894cc4b0b..8388bb61748f1 100644 --- a/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp +++ b/planning/autoware_static_centerline_generator/src/static_centerline_generator_node.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include #include diff --git a/planning/autoware_static_centerline_generator/src/utils.cpp b/planning/autoware_static_centerline_generator/src/utils.cpp index 252de0a8dde82..4ea56e10935b2 100644 --- a/planning/autoware_static_centerline_generator/src/utils.cpp +++ b/planning/autoware_static_centerline_generator/src/utils.cpp @@ -21,6 +21,9 @@ #include #include + +#include + namespace autoware::static_centerline_generator { namespace