From fd032e86926cbd191f8fdca3b154f46cadad3fd8 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Fri, 12 Apr 2024 16:59:34 +0900 Subject: [PATCH] refactor(static_centerline_generator): change the package name from static_centerline_optimizer (#6795) static_centerline_optimizer -> static_centerline_generator Signed-off-by: Takayuki Murooka --- .github/CODEOWNERS | 2 +- ...load_local_cartesian_utm_from_yaml.test.py | 2 +- .../test_node_load_local_from_yaml.test.py | 2 +- .../test_node_load_mgrs_from_yaml.test.py | 2 +- ...load_transverse_mercator_from_yaml.test.py | 2 +- planning/.pages | 2 +- .../obstacle_avoidance_planner/node.hpp | 4 +- .../path_smoother/elastic_band_smoother.hpp | 2 +- .../include/path_sampler/node.hpp | 2 +- .../CMakeLists.txt | 12 ++-- .../README.md | 6 +- .../config/common.param.yaml | 0 .../config/nearest_search.param.yaml | 0 .../static_centerline_generator.param.yaml} | 0 .../config/vehicle_info.param.yaml | 0 .../bag_ego_trajectory_based_centerline.hpp | 12 ++-- ...timization_trajectory_based_centerline.hpp | 12 ++-- .../static_centerline_generator_node.hpp} | 30 +++++----- .../type_alias.hpp | 10 ++-- .../static_centerline_generator}/utils.hpp | 12 ++-- .../launch/run_planning_server.launch.xml | 4 +- .../static_centerline_generator.launch.xml} | 6 +- .../media/rviz.png | Bin .../media/unsafe_footprints.png | Bin .../msg/PointsWithLaneId.msg | 0 .../package.xml | 4 +- .../rviz/static_centerline_generator.rviz} | 8 +-- .../scripts/app.py | 14 ++--- .../scripts/centerline_updater_helper.py | 2 +- .../bag_ego_trajectory_based_centerline.sh | 3 + ...ptimization_trajectory_based_centerline.sh | 2 +- .../bag_ego_trajectory_based_centerline.cpp | 8 +-- ...timization_trajectory_based_centerline.cpp | 10 ++-- .../src/main.cpp | 4 +- .../src/static_centerline_generator_node.cpp} | 56 +++++++++--------- .../src/utils.cpp | 6 +- .../srv/LoadMap.srv | 0 .../srv/PlanPath.srv | 2 +- .../srv/PlanRoute.srv | 0 .../test/data/bag_ego_trajectory.db3 | Bin .../test/data/lanelet2_map.osm | 0 .../test_static_centerline_generator.test.py} | 20 +++---- .../bag_ego_trajectory_based_centerline.sh | 3 - 43 files changed, 133 insertions(+), 133 deletions(-) rename planning/{static_centerline_optimizer => static_centerline_generator}/CMakeLists.txt (80%) rename planning/{static_centerline_optimizer => static_centerline_generator}/README.md (93%) rename planning/{static_centerline_optimizer => static_centerline_generator}/config/common.param.yaml (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/config/nearest_search.param.yaml (100%) rename planning/{static_centerline_optimizer/config/static_centerline_optimizer.param.yaml => static_centerline_generator/config/static_centerline_generator.param.yaml} (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/config/vehicle_info.param.yaml (100%) rename planning/{static_centerline_optimizer/include/static_centerline_optimizer => static_centerline_generator/include/static_centerline_generator}/centerline_source/bag_ego_trajectory_based_centerline.hpp (71%) rename planning/{static_centerline_optimizer/include/static_centerline_optimizer => static_centerline_generator/include/static_centerline_generator}/centerline_source/optimization_trajectory_based_centerline.hpp (81%) rename planning/{static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp => static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp} (81%) rename planning/{static_centerline_optimizer/include/static_centerline_optimizer => static_centerline_generator/include/static_centerline_generator}/type_alias.hpp (87%) rename planning/{static_centerline_optimizer/include/static_centerline_optimizer => static_centerline_generator/include/static_centerline_generator}/utils.hpp (86%) rename planning/{static_centerline_optimizer => static_centerline_generator}/launch/run_planning_server.launch.xml (54%) rename planning/{static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml => static_centerline_generator/launch/static_centerline_generator.launch.xml} (93%) rename planning/{static_centerline_optimizer => static_centerline_generator}/media/rviz.png (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/media/unsafe_footprints.png (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/msg/PointsWithLaneId.msg (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/package.xml (94%) rename planning/{static_centerline_optimizer/rviz/static_centerline_optimizer.rviz => static_centerline_generator/rviz/static_centerline_generator.rviz} (98%) rename planning/{static_centerline_optimizer => static_centerline_generator}/scripts/app.py (92%) rename planning/{static_centerline_optimizer => static_centerline_generator}/scripts/centerline_updater_helper.py (99%) create mode 100755 planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh rename planning/{static_centerline_optimizer => static_centerline_generator}/scripts/tmp/optimization_trajectory_based_centerline.sh (57%) rename planning/{static_centerline_optimizer => static_centerline_generator}/src/centerline_source/bag_ego_trajectory_based_centerline.cpp (93%) rename planning/{static_centerline_optimizer => static_centerline_generator}/src/centerline_source/optimization_trajectory_based_centerline.cpp (96%) rename planning/{static_centerline_optimizer => static_centerline_generator}/src/main.cpp (86%) rename planning/{static_centerline_optimizer/src/static_centerline_optimizer_node.cpp => static_centerline_generator/src/static_centerline_generator_node.cpp} (93%) rename planning/{static_centerline_optimizer => static_centerline_generator}/src/utils.cpp (98%) rename planning/{static_centerline_optimizer => static_centerline_generator}/srv/LoadMap.srv (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/srv/PlanPath.srv (55%) rename planning/{static_centerline_optimizer => static_centerline_generator}/srv/PlanRoute.srv (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/test/data/bag_ego_trajectory.db3 (100%) rename planning/{static_centerline_optimizer => static_centerline_generator}/test/data/lanelet2_map.osm (100%) rename planning/{static_centerline_optimizer/test/test_static_centerline_optimizer.test.py => static_centerline_generator/test/test_static_centerline_generator.test.py} (84%) delete mode 100755 planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 256b617116432..a6cdc58f03e94 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -205,7 +205,7 @@ 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_optimizer/** kosuke.takeuchi@tier4.jp takayuki.murooka@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 diff --git a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py index ec13df010c8c3..b8540550ce9da 100644 --- a/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_cartesian_utm_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py index ccec68486b79c..c7697038cc253 100644 --- a/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_local_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py index 200504d0ea18f..f75beddc6827c 100644 --- a/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py index 7883ae6aa3c99..765f3cde04679 100644 --- a/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py +++ b/map/map_projection_loader/test/test_node_load_transverse_mercator_from_yaml.test.py @@ -61,7 +61,7 @@ def generate_test_description(): LaunchDescription( [ map_projection_loader_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/planning/.pages b/planning/.pages index 1b5608f7ded57..942a5a0b32158 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 Optimizer': planning/static_centerline_optimizer + - 'Static Centerline Generator': planning/static_centerline_generator - 'API and Library': - 'Costmap Generator': planning/costmap_generator - 'External Velocity Limit Selector': planning/external_velocity_limit_selector diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 2dbefffe67738..3677e6c5fb075 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -39,12 +39,12 @@ class ObstacleAvoidancePlanner : public rclcpp::Node public: explicit ObstacleAvoidancePlanner(const rclcpp::NodeOptions & node_options); - // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. std::shared_ptr getMPTOptimizer() const { return mpt_optimizer_ptr_; } // private: -protected: // for the static_centerline_optimizer package +protected: // for the static_centerline_generator package // TODO(murooka) move this node to common class DrivingDirectionChecker { diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index eb2d651cc967d..e0bdb326adb33 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -38,7 +38,7 @@ class ElasticBandSmoother : public rclcpp::Node public: explicit ElasticBandSmoother(const rclcpp::NodeOptions & node_options); - // NOTE: This is for the static_centerline_optimizer package which utilizes the following + // NOTE: This is for the static_centerline_generator package which utilizes the following // instance. std::shared_ptr getElasticBandSmoother() const { return eb_path_smoother_ptr_; } diff --git a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp index a9002c8cf8a9f..50464f12114b0 100644 --- a/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp +++ b/planning/sampling_based_planner/path_sampler/include/path_sampler/node.hpp @@ -37,7 +37,7 @@ class PathSampler : public rclcpp::Node public: explicit PathSampler(const rclcpp::NodeOptions & node_options); -protected: // for the static_centerline_optimizer package +protected: // for the static_centerline_generator package // argument variables vehicle_info_util::VehicleInfo vehicle_info_{}; mutable DebugData debug_data_{}; diff --git a/planning/static_centerline_optimizer/CMakeLists.txt b/planning/static_centerline_generator/CMakeLists.txt similarity index 80% rename from planning/static_centerline_optimizer/CMakeLists.txt rename to planning/static_centerline_generator/CMakeLists.txt index cc33f2234a50c..991d12097cc08 100644 --- a/planning/static_centerline_optimizer/CMakeLists.txt +++ b/planning/static_centerline_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(static_centerline_optimizer) +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( - static_centerline_optimizer + static_centerline_generator "srv/LoadMap.srv" "srv/PlanRoute.srv" "srv/PlanPath.srv" @@ -21,7 +21,7 @@ autoware_package() ament_auto_add_executable(main src/main.cpp - src/static_centerline_optimizer_node.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 @@ -29,10 +29,10 @@ ament_auto_add_executable(main if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) rosidl_target_interfaces(main - static_centerline_optimizer "rosidl_typesupport_cpp") + static_centerline_generator "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( - cpp_typesupport_target static_centerline_optimizer "rosidl_typesupport_cpp") + cpp_typesupport_target static_centerline_generator "rosidl_typesupport_cpp") target_link_libraries(main "${cpp_typesupport_target}") endif() @@ -45,7 +45,7 @@ ament_auto_package( if(BUILD_TESTING) add_launch_test( - test/test_static_centerline_optimizer.test.py + test/test_static_centerline_generator.test.py TIMEOUT "30" ) install(DIRECTORY diff --git a/planning/static_centerline_optimizer/README.md b/planning/static_centerline_generator/README.md similarity index 93% rename from planning/static_centerline_optimizer/README.md rename to planning/static_centerline_generator/README.md index fd5a58ef816df..00572b754645c 100644 --- a/planning/static_centerline_optimizer/README.md +++ b/planning/static_centerline_generator/README.md @@ -1,4 +1,4 @@ -# Static Centerline Optimizer +# Static Centerline Generator ## Purpose @@ -34,7 +34,7 @@ We can run with the following command by designating `` ```sh -ros2 launch static_centerline_optimizer 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 static_centerline_optimizer static_centerline_optimizer.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_optimizer/config/common.param.yaml b/planning/static_centerline_generator/config/common.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/common.param.yaml rename to planning/static_centerline_generator/config/common.param.yaml diff --git a/planning/static_centerline_optimizer/config/nearest_search.param.yaml b/planning/static_centerline_generator/config/nearest_search.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/nearest_search.param.yaml rename to planning/static_centerline_generator/config/nearest_search.param.yaml diff --git a/planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml b/planning/static_centerline_generator/config/static_centerline_generator.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/static_centerline_optimizer.param.yaml rename to planning/static_centerline_generator/config/static_centerline_generator.param.yaml diff --git a/planning/static_centerline_optimizer/config/vehicle_info.param.yaml b/planning/static_centerline_generator/config/vehicle_info.param.yaml similarity index 100% rename from planning/static_centerline_optimizer/config/vehicle_info.param.yaml rename to planning/static_centerline_generator/config/vehicle_info.param.yaml diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/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 similarity index 71% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp index b29f222b068cc..30b2e55c36bba 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/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 @@ -12,16 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +#define STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ #include "rclcpp/rclcpp.hpp" -#include "static_centerline_optimizer/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include -namespace static_centerline_optimizer +namespace static_centerline_generator { std::vector generate_centerline_with_bag(rclcpp::Node & node); -} // namespace static_centerline_optimizer -#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ +} // namespace static_centerline_generator +#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__BAG_EGO_TRAJECTORY_BASED_CENTERLINE_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp similarity index 81% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp index 8c7c1e69e9ce6..7e7cdea31e9f1 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT -#define STATIC_CENTERLINE_OPTIMIZER__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 "static_centerline_optimizer/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include -namespace static_centerline_optimizer +namespace 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_optimizer +} // namespace static_centerline_generator // clang-format off -#endif // STATIC_CENTERLINE_OPTIMIZER__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT +#endif // STATIC_CENTERLINE_GENERATOR__CENTERLINE_SOURCE__OPTIMIZATION_TRAJECTORY_BASED_CENTERLINE_HPP_ // NOLINT // clang-format on diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp similarity index 81% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp index 5d9530a469012..c1d92c06a8e05 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/static_centerline_optimizer_node.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/static_centerline_generator_node.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ +#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_optimizer/centerline_source/optimization_trajectory_based_centerline.hpp" -#include "static_centerline_optimizer/srv/load_map.hpp" -#include "static_centerline_optimizer/srv/plan_path.hpp" -#include "static_centerline_optimizer/srv/plan_route.hpp" -#include "static_centerline_optimizer/type_alias.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" #include @@ -34,11 +34,11 @@ #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { -using static_centerline_optimizer::srv::LoadMap; -using static_centerline_optimizer::srv::PlanPath; -using static_centerline_optimizer::srv::PlanRoute; +using static_centerline_generator::srv::LoadMap; +using static_centerline_generator::srv::PlanPath; +using static_centerline_generator::srv::PlanRoute; struct CenterlineWithRoute { @@ -46,10 +46,10 @@ struct CenterlineWithRoute std::vector route_lane_ids{}; }; -class StaticCenterlineOptimizerNode : public rclcpp::Node +class StaticCenterlineGeneratorNode : public rclcpp::Node { public: - explicit StaticCenterlineOptimizerNode(const rclcpp::NodeOptions & node_options); + explicit StaticCenterlineGeneratorNode(const rclcpp::NodeOptions & node_options); void run(); private: @@ -115,5 +115,5 @@ class StaticCenterlineOptimizerNode : public rclcpp::Node // vehicle info vehicle_info_util::VehicleInfo vehicle_info_; }; -} // namespace static_centerline_optimizer -#endif // STATIC_CENTERLINE_OPTIMIZER__STATIC_CENTERLINE_OPTIMIZER_NODE_HPP_ +} // namespace static_centerline_generator +#endif // STATIC_CENTERLINE_GENERATOR__STATIC_CENTERLINE_GENERATOR_NODE_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp b/planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp similarity index 87% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/type_alias.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/type_alias.hpp index 1a700d36deaaf..2dcb9cbbd099f 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/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_OPTIMIZER__TYPE_ALIAS_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__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" @@ -26,7 +26,7 @@ #include "autoware_planning_msgs/msg/lanelet_route.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace static_centerline_optimizer +namespace 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_optimizer +} // namespace static_centerline_generator -#endif // STATIC_CENTERLINE_OPTIMIZER__TYPE_ALIAS_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR__TYPE_ALIAS_HPP_ diff --git a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp similarity index 86% rename from planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp rename to planning/static_centerline_generator/include/static_centerline_generator/utils.hpp index 04f2268aa972f..c8cf8f8b90590 100644 --- a/planning/static_centerline_optimizer/include/static_centerline_optimizer/utils.hpp +++ b/planning/static_centerline_generator/include/static_centerline_generator/utils.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ -#define STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ +#ifndef STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ +#define STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ #include "route_handler/route_handler.hpp" -#include "static_centerline_optimizer/type_alias.hpp" +#include "static_centerline_generator/type_alias.hpp" #include @@ -24,7 +24,7 @@ #include #include -namespace static_centerline_optimizer +namespace 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_optimizer +} // namespace static_centerline_generator -#endif // STATIC_CENTERLINE_OPTIMIZER__UTILS_HPP_ +#endif // STATIC_CENTERLINE_GENERATOR__UTILS_HPP_ diff --git a/planning/static_centerline_optimizer/launch/run_planning_server.launch.xml b/planning/static_centerline_generator/launch/run_planning_server.launch.xml similarity index 54% rename from planning/static_centerline_optimizer/launch/run_planning_server.launch.xml rename to planning/static_centerline_generator/launch/run_planning_server.launch.xml index 3a8b88a021dc0..1493078317458 100644 --- a/planning/static_centerline_optimizer/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_optimizer/launch/static_centerline_optimizer.launch.xml b/planning/static_centerline_generator/launch/static_centerline_generator.launch.xml similarity index 93% rename from planning/static_centerline_optimizer/launch/static_centerline_optimizer.launch.xml rename to planning/static_centerline_generator/launch/static_centerline_generator.launch.xml index ed137367f3ba4..ae71b0ae6e925 100644 --- a/planning/static_centerline_optimizer/launch/static_centerline_optimizer.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_optimizer/media/rviz.png b/planning/static_centerline_generator/media/rviz.png similarity index 100% rename from planning/static_centerline_optimizer/media/rviz.png rename to planning/static_centerline_generator/media/rviz.png diff --git a/planning/static_centerline_optimizer/media/unsafe_footprints.png b/planning/static_centerline_generator/media/unsafe_footprints.png similarity index 100% rename from planning/static_centerline_optimizer/media/unsafe_footprints.png rename to planning/static_centerline_generator/media/unsafe_footprints.png diff --git a/planning/static_centerline_optimizer/msg/PointsWithLaneId.msg b/planning/static_centerline_generator/msg/PointsWithLaneId.msg similarity index 100% rename from planning/static_centerline_optimizer/msg/PointsWithLaneId.msg rename to planning/static_centerline_generator/msg/PointsWithLaneId.msg diff --git a/planning/static_centerline_optimizer/package.xml b/planning/static_centerline_generator/package.xml similarity index 94% rename from planning/static_centerline_optimizer/package.xml rename to planning/static_centerline_generator/package.xml index eef133581622f..6573f6e439c43 100644 --- a/planning/static_centerline_optimizer/package.xml +++ b/planning/static_centerline_generator/package.xml @@ -1,9 +1,9 @@ - static_centerline_optimizer + static_centerline_generator 0.1.0 - The static_centerline_optimizer package + The static_centerline_generator package Takayuki Murooka Kosuke Takeuchi Apache License 2.0 diff --git a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz b/planning/static_centerline_generator/rviz/static_centerline_generator.rviz similarity index 98% rename from planning/static_centerline_optimizer/rviz/static_centerline_optimizer.rviz rename to planning/static_centerline_generator/rviz/static_centerline_generator.rviz index 1e7573d561ba1..62b4c9b75ec87 100644 --- a/planning/static_centerline_optimizer/rviz/static_centerline_optimizer.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_optimizer/input_centerline + Value: /static_centerline_generator/input_centerline Value: true View Drivable Area: Alpha: 0.9990000128746033 @@ -179,7 +179,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/output_centerline + Value: /static_centerline_generator/output_centerline Value: true View Footprint: Alpha: 1 @@ -222,7 +222,7 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/raw_centerline + Value: /static_centerline_generator/debug/raw_centerline Value: false View Drivable Area: Alpha: 0.9990000128746033 @@ -268,7 +268,7 @@ Visualization Manager: Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable - Value: /static_centerline_optimizer/debug/unsafe_footprints + Value: /static_centerline_generator/debug/unsafe_footprints Value: true Enabled: false Name: debug diff --git a/planning/static_centerline_optimizer/scripts/app.py b/planning/static_centerline_generator/scripts/app.py similarity index 92% rename from planning/static_centerline_optimizer/scripts/app.py rename to planning/static_centerline_generator/scripts/app.py index 90610755e828a..c1cb0473da040 100755 --- a/planning/static_centerline_optimizer/scripts/app.py +++ b/planning/static_centerline_generator/scripts/app.py @@ -23,12 +23,12 @@ from flask_cors import CORS import rclpy from rclpy.node import Node -from static_centerline_optimizer.srv import LoadMap -from static_centerline_optimizer.srv import PlanPath -from static_centerline_optimizer.srv import PlanRoute +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_optimizer_http_server") +node = Node("static_centerline_generator_http_server") app = Flask(__name__) CORS(app) @@ -51,7 +51,7 @@ def get_map(): map_id = map_uuid # create client - cli = create_client(LoadMap, "/planning/static_centerline_optimizer/load_map") + cli = create_client(LoadMap, "/planning/static_centerline_generator/load_map") # request map loading req = LoadMap.Request(map=data["map"]) @@ -85,7 +85,7 @@ def post_planned_route(): print("map_id is not correct.") # create client - cli = create_client(PlanRoute, "/planning/static_centerline_optimizer/plan_route") + cli = create_client(PlanRoute, "/planning/static_centerline_generator/plan_route") # request route planning req = PlanRoute.Request( @@ -123,7 +123,7 @@ def post_planned_path(): print("map_id is not correct.") # create client - cli = create_client(PlanPath, "/planning/static_centerline_optimizer/plan_path") + cli = create_client(PlanPath, "/planning/static_centerline_generator/plan_path") # request path planning route_lane_ids = [eval(i) for i in request.args.getlist("route[]")] diff --git a/planning/static_centerline_optimizer/scripts/centerline_updater_helper.py b/planning/static_centerline_generator/scripts/centerline_updater_helper.py similarity index 99% rename from planning/static_centerline_optimizer/scripts/centerline_updater_helper.py rename to planning/static_centerline_generator/scripts/centerline_updater_helper.py index 00a646406f14f..fec3d21d20bec 100755 --- a/planning/static_centerline_optimizer/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_optimizer/output_whole_centerline", + "/static_centerline_generator/output_whole_centerline", self.onWholeCenterline, transient_local_qos, ) 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 new file mode 100755 index 0000000000000..30ed667dd3732 --- /dev/null +++ b/planning/static_centerline_generator/scripts/tmp/bag_ego_trajectory_based_centerline.sh @@ -0,0 +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 diff --git a/planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh b/planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh similarity index 57% rename from planning/static_centerline_optimizer/scripts/tmp/optimization_trajectory_based_centerline.sh rename to planning/static_centerline_generator/scripts/tmp/optimization_trajectory_based_centerline.sh index 3967c7a4ca523..809bbb46a179e 100755 --- a/planning/static_centerline_optimizer/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_optimizer static_centerline_optimizer.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 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_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp similarity index 93% rename from planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp rename to planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp index 571e98cdbe1c3..4e541b1aff527 100644 --- a/planning/static_centerline_optimizer/src/centerline_source/bag_ego_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/bag_ego_trajectory_based_centerline.cpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp" +#include "static_centerline_generator/centerline_source/bag_ego_trajectory_based_centerline.hpp" #include "rclcpp/serialization.hpp" #include "rosbag2_cpp/reader.hpp" -#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" #include -namespace static_centerline_optimizer +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 static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp similarity index 96% rename from planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp rename to planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 20b2027f39d54..7980ae4e8d2d7 100644 --- a/planning/static_centerline_optimizer/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "static_centerline_optimizer/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" #include "obstacle_avoidance_planner/node.hpp" #include "path_smoother/elastic_band_smoother.hpp" -#include "static_centerline_optimizer/static_centerline_optimizer_node.hpp" -#include "static_centerline_optimizer/utils.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" +#include "static_centerline_generator/utils.hpp" #include "tier4_autoware_utils/ros/parameter.hpp" -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace { @@ -180,4 +180,4 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra return whole_optimized_traj_points; } -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/src/main.cpp b/planning/static_centerline_generator/src/main.cpp similarity index 86% rename from planning/static_centerline_optimizer/src/main.cpp rename to planning/static_centerline_generator/src/main.cpp index 96c139fd7ff93..981cf54fc9274 100644 --- a/planning/static_centerline_optimizer/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_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" int main(int argc, char * argv[]) { @@ -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_optimizer/src/static_centerline_optimizer_node.cpp b/planning/static_centerline_generator/src/static_centerline_generator_node.cpp similarity index 93% rename from planning/static_centerline_optimizer/src/static_centerline_optimizer_node.cpp rename to planning/static_centerline_generator/src/static_centerline_generator_node.cpp index 073d30f5b5a39..ec7d6278e346d 100644 --- a/planning/static_centerline_optimizer/src/static_centerline_optimizer_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_optimizer/static_centerline_optimizer_node.hpp" +#include "static_centerline_generator/static_centerline_generator_node.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include "lanelet2_extension/utility/query.hpp" @@ -21,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 "static_centerline_optimizer/centerline_source/bag_ego_trajectory_based_centerline.hpp" -#include "static_centerline_optimizer/msg/points_with_lane_id.hpp" -#include "static_centerline_optimizer/type_alias.hpp" -#include "static_centerline_optimizer/utils.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" #include "tier4_autoware_utils/ros/parameter.hpp" @@ -53,7 +53,7 @@ #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace { @@ -165,9 +165,9 @@ std::vector resample_trajectory_points( } } // namespace -StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( +StaticCenterlineGeneratorNode::StaticCenterlineGeneratorNode( const rclcpp::NodeOptions & node_options) -: Node("static_centerline_optimizer", node_options) +: Node("static_centerline_generator", node_options) { // publishers pub_map_bin_ = @@ -215,21 +215,21 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( // services callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_load_map_ = create_service( - "/planning/static_centerline_optimizer/load_map", + "/planning/static_centerline_generator/load_map", std::bind( - &StaticCenterlineOptimizerNode::on_load_map, this, std::placeholders::_1, + &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_optimizer/plan_route", + "/planning/static_centerline_generator/plan_route", std::bind( - &StaticCenterlineOptimizerNode::on_plan_route, this, std::placeholders::_1, + &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_optimizer/plan_path", + "/planning/static_centerline_generator/plan_path", std::bind( - &StaticCenterlineOptimizerNode::on_plan_path, this, std::placeholders::_1, + &StaticCenterlineGeneratorNode::on_plan_path, this, std::placeholders::_1, std::placeholders::_2), rmw_qos_profile_services_default, callback_group_); @@ -245,11 +245,11 @@ StaticCenterlineOptimizerNode::StaticCenterlineOptimizerNode( return CenterlineSource::BagEgoTrajectoryBase; } throw std::logic_error( - "The centerline source is not supported in static_centerline_optimizer."); + "The centerline source is not supported in static_centerline_generator."); }(); } -void StaticCenterlineOptimizerNode::update_centerline_range( +void StaticCenterlineGeneratorNode::update_centerline_range( const int traj_start_index, const int traj_end_index) { if (!centerline_with_route_ || traj_range_indices_.second + 1 < traj_start_index) { @@ -267,7 +267,7 @@ void StaticCenterlineOptimizerNode::update_centerline_range( motion_utils::convertToTrajectory(selected_centerline, create_header(this->now()))); } -void StaticCenterlineOptimizerNode::run() +void StaticCenterlineGeneratorNode::run() { // declare planning setting parameters const auto lanelet2_input_file_path = declare_parameter("lanelet2_input_file_path"); @@ -283,7 +283,7 @@ void StaticCenterlineOptimizerNode::run() centerline_with_route_ = centerline_with_route; } -CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_route() +CenterlineWithRoute StaticCenterlineGeneratorNode::generate_centerline_with_route() { if (!route_handler_ptr_) { RCLCPP_ERROR(get_logger(), "Route handler is not ready. Return empty trajectory."); @@ -317,7 +317,7 @@ CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_rout return CenterlineWithRoute{bag_centerline, route_lane_ids}; } throw std::logic_error( - "The centerline source is not supported in static_centerline_optimizer."); + "The centerline source is not supported in static_centerline_generator."); }(); // resample @@ -334,7 +334,7 @@ CenterlineWithRoute StaticCenterlineOptimizerNode::generate_centerline_with_rout return centerline_with_route; } -void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_file_path) +void StaticCenterlineGeneratorNode::load_map(const std::string & lanelet2_input_file_path) { // load map by the map_loader package map_bin_ptr_ = [&]() -> HADMapBin::ConstSharedPtr { @@ -381,7 +381,7 @@ void StaticCenterlineOptimizerNode::load_map(const std::string & lanelet2_input_ route_handler_ptr_->setMap(*map_bin_ptr_); } -void StaticCenterlineOptimizerNode::on_load_map( +void StaticCenterlineGeneratorNode::on_load_map( const LoadMap::Request::SharedPtr request, const LoadMap::Response::SharedPtr response) { const std::string tmp_lanelet2_input_file_path = "/tmp/input_lanelet2_map.osm"; @@ -402,7 +402,7 @@ void StaticCenterlineOptimizerNode::on_load_map( response->message = "InvalidMapFormat"; } -std::vector StaticCenterlineOptimizerNode::plan_route( +std::vector StaticCenterlineGeneratorNode::plan_route( const lanelet::Id start_lanelet_id, const lanelet::Id end_lanelet_id) { if (!map_bin_ptr_ || !route_handler_ptr_) { @@ -442,7 +442,7 @@ std::vector StaticCenterlineOptimizerNode::plan_route( return route_lane_ids; } -void StaticCenterlineOptimizerNode::on_plan_route( +void StaticCenterlineGeneratorNode::on_plan_route( const PlanRoute::Request::SharedPtr request, const PlanRoute::Response::SharedPtr response) { if (!map_bin_ptr_ || !route_handler_ptr_) { @@ -475,7 +475,7 @@ void StaticCenterlineOptimizerNode::on_plan_route( response->lane_ids = lane_ids; } -void StaticCenterlineOptimizerNode::on_plan_path( +void StaticCenterlineGeneratorNode::on_plan_path( const PlanPath::Request::SharedPtr request, const PlanPath::Response::SharedPtr response) { if (!route_handler_ptr_) { @@ -533,7 +533,7 @@ void StaticCenterlineOptimizerNode::on_plan_path( if (!current_lanelet_points.empty()) { // register points with lane_id - static_centerline_optimizer::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); @@ -548,7 +548,7 @@ void StaticCenterlineOptimizerNode::on_plan_path( response->message = ""; } -void StaticCenterlineOptimizerNode::evaluate( +void StaticCenterlineGeneratorNode::evaluate( const std::vector & route_lane_ids, const std::vector & optimized_traj_points) { @@ -618,7 +618,7 @@ void StaticCenterlineOptimizerNode::evaluate( RCLCPP_INFO(get_logger(), "Minimum distance to road is %f [m]", min_dist); } -void StaticCenterlineOptimizerNode::save_map( +void StaticCenterlineGeneratorNode::save_map( const std::string & lanelet2_output_file_path, const CenterlineWithRoute & centerline_with_route) { if (!route_handler_ptr_) { @@ -636,4 +636,4 @@ void StaticCenterlineOptimizerNode::save_map( lanelet::write(lanelet2_output_file_path, *original_map_ptr_, *map_projector_); RCLCPP_INFO(get_logger(), "Saved map."); } -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_generator/src/utils.cpp similarity index 98% rename from planning/static_centerline_optimizer/src/utils.cpp rename to planning/static_centerline_generator/src/utils.cpp index 9448677d1bbae..ddfd3e11036ce 100644 --- a/planning/static_centerline_optimizer/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_optimizer/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" @@ -21,7 +21,7 @@ #include #include -namespace static_centerline_optimizer +namespace static_centerline_generator { namespace { @@ -228,4 +228,4 @@ MarkerArray create_distance_text_marker( return marker_array; } } // namespace utils -} // namespace static_centerline_optimizer +} // namespace static_centerline_generator diff --git a/planning/static_centerline_optimizer/srv/LoadMap.srv b/planning/static_centerline_generator/srv/LoadMap.srv similarity index 100% rename from planning/static_centerline_optimizer/srv/LoadMap.srv rename to planning/static_centerline_generator/srv/LoadMap.srv diff --git a/planning/static_centerline_optimizer/srv/PlanPath.srv b/planning/static_centerline_generator/srv/PlanPath.srv similarity index 55% rename from planning/static_centerline_optimizer/srv/PlanPath.srv rename to planning/static_centerline_generator/srv/PlanPath.srv index 644a4ea013557..7d823b4eccbf9 100644 --- a/planning/static_centerline_optimizer/srv/PlanPath.srv +++ b/planning/static_centerline_generator/srv/PlanPath.srv @@ -1,6 +1,6 @@ uint32 map_id int64[] route --- -static_centerline_optimizer/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_optimizer/srv/PlanRoute.srv b/planning/static_centerline_generator/srv/PlanRoute.srv similarity index 100% rename from planning/static_centerline_optimizer/srv/PlanRoute.srv rename to planning/static_centerline_generator/srv/PlanRoute.srv diff --git a/planning/static_centerline_optimizer/test/data/bag_ego_trajectory.db3 b/planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 similarity index 100% rename from planning/static_centerline_optimizer/test/data/bag_ego_trajectory.db3 rename to planning/static_centerline_generator/test/data/bag_ego_trajectory.db3 diff --git a/planning/static_centerline_optimizer/test/data/lanelet2_map.osm b/planning/static_centerline_generator/test/data/lanelet2_map.osm similarity index 100% rename from planning/static_centerline_optimizer/test/data/lanelet2_map.osm rename to planning/static_centerline_generator/test/data/lanelet2_map.osm diff --git a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py b/planning/static_centerline_generator/test/test_static_centerline_generator.test.py similarity index 84% rename from planning/static_centerline_optimizer/test/test_static_centerline_optimizer.test.py rename to planning/static_centerline_generator/test/test_static_centerline_generator.test.py index 9f0a5df7f3fcc..29ee49a11e3b3 100644 --- a/planning/static_centerline_optimizer/test/test_static_centerline_optimizer.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_optimizer"), "test/data/lanelet2_map.osm" + get_package_share_directory("static_centerline_generator"), "test/data/lanelet2_map.osm" ) - static_centerline_optimizer_node = Node( - package="static_centerline_optimizer", + static_centerline_generator_node = Node( + package="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_optimizer"), - "config/static_centerline_optimizer.param.yaml", + get_package_share_directory("static_centerline_generator"), + "config/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_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/common.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/nearest_search.param.yaml", ), os.path.join( - get_package_share_directory("static_centerline_optimizer"), + get_package_share_directory("static_centerline_generator"), "config/vehicle_info.param.yaml", ), ], @@ -93,8 +93,8 @@ def generate_test_description(): return ( LaunchDescription( [ - static_centerline_optimizer_node, - # Start test after 1s - gives time for the static_centerline_optimizer to finish initialization + static_centerline_generator_node, + # Start test after 1s - gives time for the static_centerline_generator to finish initialization launch.actions.TimerAction( period=1.0, actions=[launch_testing.actions.ReadyToTest()] ), diff --git a/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh b/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh deleted file mode 100755 index 8164b521553c8..0000000000000 --- a/planning/static_centerline_optimizer/scripts/tmp/bag_ego_trajectory_based_centerline.sh +++ /dev/null @@ -1,3 +0,0 @@ -#!/bin/bash - -ros2 launch static_centerline_optimizer static_centerline_optimizer.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_optimizer --share)/test/data/bag_ego_trajectory.db3" vehicle_model:=lexus