diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 2cf484c9fbe34..278329cd3c725 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -171,13 +171,13 @@ planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp s planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_traffic_light_module/** mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp @@ -189,7 +189,7 @@ planning/obstacle_stop_planner/** berkay@leodrive.ai bnk@leodrive.ai satoshi.ota planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp zulfaqar.azmi@tier4.jp -planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp +planning/autoware_planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp planning/route_handler/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 154a3019f1c17..b093bc6295e65 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -156,7 +156,7 @@ /> - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index be85ee704ff95..6a7e00154256a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -19,7 +19,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index cf96cd39043ce..b26b6d4fb6276 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,20 +58,20 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_external_velocity_limit_selector autoware_path_optimizer + autoware_planning_topic_converter autoware_remaining_distance_time_calculator autoware_velocity_smoother behavior_path_planner costmap_generator external_cmd_selector - external_velocity_limit_selector freespace_planner glog_component mission_planner obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_topic_converter planning_validator scenario_selector surround_obstacle_checker diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 24145a7920d91..e8428788820b2 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -52,6 +52,6 @@ class StopFilter : public rclcpp::Node /** * @brief set odometry measurement */ - void callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); + void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg); }; #endif // STOP_FILTER__STOP_FILTER_HPP_ diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index 4d6b2c6240867..c1c4de2fb6b6e 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -31,13 +31,13 @@ StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) wz_threshold_ = declare_parameter("wz_threshold"); sub_odom_ = create_subscription( - "input/odom", 1, std::bind(&StopFilter::callbackOdometry, this, _1)); + "input/odom", 1, std::bind(&StopFilter::callback_odometry, this, _1)); pub_odom_ = create_publisher("output/odom", 1); pub_stop_flag_ = create_publisher("debug/stop_flag", 1); } -void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) +void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg) { tier4_debug_msgs::msg::BoolStamped stop_flag_msg; stop_flag_msg.stamp = msg->header.stamp; diff --git a/planning/.pages b/planning/.pages index 849d117b73b71..4966ff7dc0bb3 100644 --- a/planning/.pages +++ b/planning/.pages @@ -30,7 +30,7 @@ nav: - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module - 'Out of Lane': planning/behavior_velocity_out_of_lane_module - - 'Run Out': planning/behavior_velocity_run_out_module + - 'Run Out': planning/autoware_behavior_velocity_run_out_module - 'Speed Bump': planning/behavior_velocity_speed_bump_module - 'Stop Line': planning/behavior_velocity_stop_line_module - 'Traffic Light': planning/behavior_velocity_traffic_light_module @@ -74,7 +74,7 @@ nav: - '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 + - 'External Velocity Limit Selector': planning/autoware_external_velocity_limit_selector - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface - 'Route Handler': planning/route_handler - 'RTC Interface': planning/rtc_interface @@ -86,5 +86,5 @@ nav: - 'Stop Reason Visualizer': https://github.com/autowarefoundation/autoware_tools/blob/main/planning/planning_debug_tools/doc-stop-reason-visualizer.md - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - - 'Planning Topic Converter': planning/planning_topic_converter + - 'Planning Topic Converter': planning/autoware_planning_topic_converter - 'Planning Validator': planning/planning_validator diff --git a/planning/autoware_behavior_velocity_planner/README.md b/planning/autoware_behavior_velocity_planner/README.md index 0bbc2fad3c5a8..bcef46e90c1ec 100644 --- a/planning/autoware_behavior_velocity_planner/README.md +++ b/planning/autoware_behavior_velocity_planner/README.md @@ -18,7 +18,7 @@ It loads modules as plugins. Please refer to the links listed below for detail o - [Traffic Light](../behavior_velocity_traffic_light_module/README.md) - [Occlusion Spot](../behavior_velocity_occlusion_spot_module/README.md) - [No Stopping Area](../behavior_velocity_no_stopping_area_module/README.md) -- [Run Out](../behavior_velocity_run_out_module/README.md) +- [Run Out](../autoware_behavior_velocity_run_out_module/README.md) - [Speed Bump](../behavior_velocity_speed_bump_module/README.md) - [Out of Lane](../behavior_velocity_out_of_lane_module/README.md) diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index 3d51c6b855eb7..fff1fb8ff91c0 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -65,6 +65,7 @@ ament_cmake_ros ament_lint_auto + autoware_behavior_velocity_run_out_module autoware_behavior_velocity_virtual_traffic_light_module autoware_lint_common behavior_velocity_blind_spot_module @@ -75,7 +76,6 @@ behavior_velocity_no_stopping_area_module behavior_velocity_occlusion_spot_module behavior_velocity_out_of_lane_module - behavior_velocity_run_out_module behavior_velocity_speed_bump_module behavior_velocity_stop_line_module behavior_velocity_traffic_light_module diff --git a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp index 7cbe0f65a1fc2..e741d054afbaf 100644 --- a/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp +++ b/planning/autoware_behavior_velocity_planner/test/src/test_node_interface.cpp @@ -79,7 +79,7 @@ std::shared_ptr generateNode() module_names.emplace_back("behavior_velocity_planner::NoStoppingAreaModulePlugin"); module_names.emplace_back("behavior_velocity_planner::StopLineModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OcclusionSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::RunOutModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::RunOutModulePlugin"); module_names.emplace_back("behavior_velocity_planner::SpeedBumpModulePlugin"); module_names.emplace_back("behavior_velocity_planner::OutOfLaneModulePlugin"); module_names.emplace_back("behavior_velocity_planner::NoDrivableLaneModulePlugin"); @@ -103,7 +103,7 @@ std::shared_ptr generateNode() get_behavior_velocity_module_config_no_prefix("intersection"), get_behavior_velocity_module_config_no_prefix("no_stopping_area"), get_behavior_velocity_module_config_no_prefix("occlusion_spot"), - get_behavior_velocity_module_config_no_prefix("run_out"), + get_behavior_velocity_module_config("run_out"), get_behavior_velocity_module_config_no_prefix("speed_bump"), get_behavior_velocity_module_config_no_prefix("stop_line"), get_behavior_velocity_module_config_no_prefix("traffic_light"), diff --git a/planning/behavior_velocity_run_out_module/CMakeLists.txt b/planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt similarity index 89% rename from planning/behavior_velocity_run_out_module/CMakeLists.txt rename to planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt index 3ee589ba69740..bcbe4ea12a643 100644 --- a/planning/behavior_velocity_run_out_module/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_run_out_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_run_out_module) +project(autoware_behavior_velocity_run_out_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_run_out_module/README.md b/planning/autoware_behavior_velocity_run_out_module/README.md similarity index 100% rename from planning/behavior_velocity_run_out_module/README.md rename to planning/autoware_behavior_velocity_run_out_module/README.md diff --git a/planning/behavior_velocity_run_out_module/config/run_out.param.yaml b/planning/autoware_behavior_velocity_run_out_module/config/run_out.param.yaml similarity index 100% rename from planning/behavior_velocity_run_out_module/config/run_out.param.yaml rename to planning/autoware_behavior_velocity_run_out_module/config/run_out.param.yaml diff --git a/planning/behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg b/planning/autoware_behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/calculate_expected_target_velocity.svg diff --git a/planning/behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg b/planning/autoware_behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/collision_detection_for_shape.svg diff --git a/planning/behavior_velocity_run_out_module/docs/collision_points.svg b/planning/autoware_behavior_velocity_run_out_module/docs/collision_points.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/collision_points.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/collision_points.svg diff --git a/planning/behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg b/planning/autoware_behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/create_dynamic_obstacle.svg diff --git a/planning/behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg b/planning/autoware_behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/create_polygon_on_path_point.svg diff --git a/planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg b/planning/autoware_behavior_velocity_run_out_module/docs/ego_cut_line.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/ego_cut_line.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/ego_cut_line.svg diff --git a/planning/behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg b/planning/autoware_behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/exclude_obstacles_by_partition.svg diff --git a/planning/behavior_velocity_run_out_module/docs/insert_velocity.svg b/planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/insert_velocity.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity.svg diff --git a/planning/behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg b/planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/insert_velocity_to_approach.svg diff --git a/planning/behavior_velocity_run_out_module/docs/run_out_overview.svg b/planning/autoware_behavior_velocity_run_out_module/docs/run_out_overview.svg similarity index 100% rename from planning/behavior_velocity_run_out_module/docs/run_out_overview.svg rename to planning/autoware_behavior_velocity_run_out_module/docs/run_out_overview.svg diff --git a/planning/behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml similarity index 92% rename from planning/behavior_velocity_run_out_module/package.xml rename to planning/autoware_behavior_velocity_run_out_module/package.xml index 40e55b3de1a17..cb2c2df58ffe2 100644 --- a/planning/behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_run_out_module + autoware_behavior_velocity_run_out_module 0.1.0 - The behavior_velocity_run_out_module package + The autoware_behavior_velocity_run_out_module package Tomohito Ando Makoto Kurihara diff --git a/planning/autoware_behavior_velocity_run_out_module/plugins.xml b/planning/autoware_behavior_velocity_run_out_module/plugins.xml new file mode 100644 index 0000000000000..1ddcaf8e620e1 --- /dev/null +++ b/planning/autoware_behavior_velocity_run_out_module/plugins.xml @@ -0,0 +1,3 @@ + + + diff --git a/planning/behavior_velocity_run_out_module/src/debug.cpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/debug.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/debug.cpp index 3f6935365c1a4..129d5299385fd 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.cpp @@ -28,7 +28,7 @@ using tier4_autoware_utils::createMarkerOrientation; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -368,4 +368,4 @@ motion_utils::VirtualWalls RunOutModule::createVirtualWalls() return debug_ptr_->createVirtualWalls(); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/debug.hpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/debug.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/debug.hpp index fbf682279e363..3c6d475950e7c 100644 --- a/planning/behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -24,8 +24,9 @@ #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::Polygon2d; using sensor_msgs::msg::PointCloud2; using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_debug_msgs::msg::Int32Stamped; @@ -149,6 +150,6 @@ class RunOutDebug double height_{0}; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index edf1d8cf48b8e..d204c7ec6f2fc 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -30,8 +30,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::Point2d; +using ::behavior_velocity_planner::splineInterpolate; namespace { // create quaternion facing to the nearest trajectory point @@ -600,4 +602,4 @@ void DynamicObstacleCreatorForPoints::onSynchronizedPointCloud( std::lock_guard lock(mutex_); obstacle_points_map_filtered_ = lateral_nearest_points; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp similarity index 96% rename from planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index c7d8ef674aafd..b7ed815829a2b 100644 --- a/planning/behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -42,11 +42,12 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; +using ::behavior_velocity_planner::PlannerData; using run_out_utils::DynamicObstacle; using run_out_utils::DynamicObstacleData; using run_out_utils::DynamicObstacleParam; @@ -55,6 +56,7 @@ using run_out_utils::PredictedPath; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using PathPointsWithLaneId = std::vector; +using ::behavior_velocity_planner::Polygons2d; /** * @brief base class for creating dynamic obstacles from multiple types of input @@ -171,6 +173,6 @@ class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator pcl::PointCloud obstacle_points_map_filtered_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // DYNAMIC_OBSTACLE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/manager.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index fb6a70070749a..5ee50863fd162 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -20,10 +20,10 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; - RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { @@ -198,8 +198,9 @@ void RunOutModuleManager::setDynamicObstacleCreator( break; } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::RunOutModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::RunOutModulePlugin, + behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_run_out_module/src/manager.hpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp similarity index 86% rename from planning/behavior_velocity_run_out_module/src/manager.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/manager.hpp index ad35caf98149a..f0c49b99c99e5 100644 --- a/planning/behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -23,8 +23,11 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::PluginWrapper; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::SceneModuleManagerInterface; class RunOutModuleManager : public SceneModuleManagerInterface { public: @@ -49,6 +52,6 @@ class RunOutModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp similarity index 90% rename from planning/behavior_velocity_run_out_module/src/path_utils.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp index be7db13cd3ca3..346a10646e6bc 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.cpp @@ -15,7 +15,7 @@ #include "path_utils.hpp" #include -namespace behavior_velocity_planner::run_out_utils +namespace autoware::behavior_velocity_planner::run_out_utils { geometry_msgs::msg::Point findLongitudinalNearestPoint( const std::vector & points, @@ -36,4 +36,4 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( return min_dist_point; } -} // namespace behavior_velocity_planner::run_out_utils +} // namespace autoware::behavior_velocity_planner::run_out_utils diff --git a/planning/behavior_velocity_run_out_module/src/path_utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp similarity index 92% rename from planning/behavior_velocity_run_out_module/src/path_utils.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp index 8867778520bc7..62591af27af11 100644 --- a/planning/behavior_velocity_run_out_module/src/path_utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/path_utils.hpp @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -36,5 +36,5 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint( const std::vector & target_points); } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/scene.cpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp similarity index 99% rename from planning/behavior_velocity_run_out_module/src/scene.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/scene.cpp index de50b3161ea6f..351ed8e7a09b8 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -34,10 +34,14 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; +using ::behavior_velocity_planner::PlanningBehavior; using object_recognition_utils::convertLabelToString; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; +using ::behavior_velocity_planner::getCrosswalksOnPath; +using ::behavior_velocity_planner::Polygon2d; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, @@ -1031,4 +1035,4 @@ bool RunOutModule::isMomentaryDetection() return elapsed_time_since_detection < planner_param_.ignore_momentary_detection.time_threshold; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/scene.hpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp similarity index 95% rename from planning/behavior_velocity_run_out_module/src/scene.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/scene.hpp index 2f797a44d06f7..85da1340a5a5d 100644 --- a/planning/behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::PredictedObjects; using run_out_utils::PlannerParam; @@ -39,6 +39,11 @@ using tier4_debug_msgs::msg::Float32Stamped; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; using BasicPolygons2d = std::vector; +using ::behavior_velocity_planner::PathWithLaneId; +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::Polygon2d; +using ::behavior_velocity_planner::SceneModuleInterface; +using ::behavior_velocity_planner::StopReason; class RunOutModule : public SceneModuleInterface { @@ -177,6 +182,6 @@ class RunOutModule : public SceneModuleInterface bool isMomentaryDetection(); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/state_machine.cpp b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp similarity index 96% rename from planning/behavior_velocity_run_out_module/src/state_machine.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp index 322264cad8501..484a7cdcd95e3 100644 --- a/planning/behavior_velocity_run_out_module/src/state_machine.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.cpp @@ -14,7 +14,7 @@ #include "state_machine.hpp" -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -107,4 +107,4 @@ void StateMachine::updateState(const StateInput & state_input, rclcpp::Clock & c } } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/state_machine.hpp b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp similarity index 92% rename from planning/behavior_velocity_run_out_module/src/state_machine.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp index 7c0de8078f435..b88aa821548bb 100644 --- a/planning/behavior_velocity_run_out_module/src/state_machine.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/state_machine.hpp @@ -19,7 +19,7 @@ #include -namespace behavior_velocity_planner::run_out_utils +namespace autoware::behavior_velocity_planner::run_out_utils { class StateMachine @@ -53,6 +53,6 @@ class StateMachine std::optional prev_obstacle_{}; std::optional target_obstacle_{}; }; -} // namespace behavior_velocity_planner::run_out_utils +} // namespace autoware::behavior_velocity_planner::run_out_utils #endif // STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_run_out_module/src/utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp similarity index 98% rename from planning/behavior_velocity_run_out_module/src/utils.cpp rename to planning/autoware_behavior_velocity_run_out_module/src/utils.cpp index c6eda901a1424..ec1cd460e1bb0 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -33,8 +33,11 @@ #endif #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { +using ::behavior_velocity_planner::DetectionRange; +using ::behavior_velocity_planner::PathPointWithLaneId; +namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace run_out_utils { Polygon2d createBoostPolyFromMsg(const std::vector & input_poly) @@ -450,4 +453,4 @@ Polygons2d createMandatoryDetectionAreaPolygon( } } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp similarity index 97% rename from planning/behavior_velocity_run_out_module/src/utils.hpp rename to planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 9208e57dcdc7f..10f856b257a61 100644 --- a/planning/behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace run_out_utils { @@ -38,6 +38,8 @@ using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::Shape; using autoware_planning_msgs::msg::PathPoint; +using ::behavior_velocity_planner::PlannerData; +using ::behavior_velocity_planner::Polygons2d; using tier4_autoware_utils::Box2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; @@ -271,5 +273,5 @@ Polygons2d createMandatoryDetectionAreaPolygon( const PathWithLaneId & path, const PlannerData & planner_data, const PlannerParam & planner_param); } // namespace run_out_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTILS_HPP_ diff --git a/planning/external_velocity_limit_selector/CMakeLists.txt b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt similarity index 73% rename from planning/external_velocity_limit_selector/CMakeLists.txt rename to planning/autoware_external_velocity_limit_selector/CMakeLists.txt index db83dcf3cad4a..40f0ebb2a3eba 100644 --- a/planning/external_velocity_limit_selector/CMakeLists.txt +++ b/planning/autoware_external_velocity_limit_selector/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(external_velocity_limit_selector) +project(autoware_external_velocity_limit_selector) find_package(autoware_cmake REQUIRED) autoware_package() @@ -9,7 +9,7 @@ ament_auto_add_library(external_velocity_limit_selector_node SHARED ) rclcpp_components_register_node(external_velocity_limit_selector_node - PLUGIN "ExternalVelocityLimitSelectorNode" + PLUGIN "autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode" EXECUTABLE external_velocity_limit_selector ) diff --git a/planning/external_velocity_limit_selector/README.md b/planning/autoware_external_velocity_limit_selector/README.md similarity index 100% rename from planning/external_velocity_limit_selector/README.md rename to planning/autoware_external_velocity_limit_selector/README.md diff --git a/planning/external_velocity_limit_selector/config/default.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default.param.yaml diff --git a/planning/external_velocity_limit_selector/config/default_common.param.yaml b/planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml similarity index 100% rename from planning/external_velocity_limit_selector/config/default_common.param.yaml rename to planning/autoware_external_velocity_limit_selector/config/default_common.param.yaml diff --git a/planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png b/planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png similarity index 100% rename from planning/external_velocity_limit_selector/image/external_velocity_limit_selector.png rename to planning/autoware_external_velocity_limit_selector/image/external_velocity_limit_selector.png diff --git a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp similarity index 87% rename from planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp rename to planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp index 93fad135ac678..e4043f1aeb986 100644 --- a/planning/external_velocity_limit_selector/include/external_velocity_limit_selector/external_velocity_limit_selector_node.hpp +++ b/planning/autoware_external_velocity_limit_selector/include/autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ -#define EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#ifndef AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#define AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ #include @@ -26,6 +26,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + using tier4_debug_msgs::msg::StringStamped; using tier4_planning_msgs::msg::VelocityLimit; using tier4_planning_msgs::msg::VelocityLimitClearCommand; @@ -77,5 +80,6 @@ class ExternalVelocityLimitSelectorNode : public rclcpp::Node VelocityLimit hardest_limit_{}; VelocityLimitTable velocity_limit_table_; }; +} // namespace autoware::external_velocity_limit_selector -#endif // EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ +#endif // AUTOWARE_EXTERNAL_VELOCITY_LIMIT_SELECTOR__EXTERNAL_VELOCITY_LIMIT_SELECTOR_NODE_HPP_ diff --git a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml similarity index 74% rename from planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml rename to planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml index 5ef089f3d3ee7..42c8709b6663a 100644 --- a/planning/external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml +++ b/planning/autoware_external_velocity_limit_selector/launch/external_velocity_limit_selector.launch.xml @@ -1,7 +1,7 @@ - - + + @@ -10,7 +10,7 @@ - + diff --git a/planning/external_velocity_limit_selector/package.xml b/planning/autoware_external_velocity_limit_selector/package.xml similarity index 88% rename from planning/external_velocity_limit_selector/package.xml rename to planning/autoware_external_velocity_limit_selector/package.xml index e83966c7b876d..803e32237e938 100644 --- a/planning/external_velocity_limit_selector/package.xml +++ b/planning/autoware_external_velocity_limit_selector/package.xml @@ -1,9 +1,9 @@ - external_velocity_limit_selector + autoware_external_velocity_limit_selector 0.1.0 - The external_velocity_limit_selector ROS 2 package + The autoware_external_velocity_limit_selector ROS 2 package Satoshi Ota Shinnosuke Hirakawa Shumpei Wakabayashi diff --git a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp similarity index 96% rename from planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp rename to planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp index 5e557167ac3e1..008b58df303b1 100644 --- a/planning/external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp +++ b/planning/autoware_external_velocity_limit_selector/src/external_velocity_limit_selector_node.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" +#include "autoware_external_velocity_limit_selector/external_velocity_limit_selector_node.hpp" #include #include @@ -21,6 +21,9 @@ #include #include +namespace autoware::external_velocity_limit_selector +{ + namespace { VelocityLimit getHardestLimit( @@ -236,6 +239,8 @@ void ExternalVelocityLimitSelectorNode::updateVelocityLimit() hardest_limit_ = getHardestLimit(velocity_limit_table_, node_param_); } +} // namespace autoware::external_velocity_limit_selector #include -RCLCPP_COMPONENTS_REGISTER_NODE(ExternalVelocityLimitSelectorNode) +RCLCPP_COMPONENTS_REGISTER_NODE( + autoware::external_velocity_limit_selector::ExternalVelocityLimitSelectorNode) diff --git a/planning/planning_topic_converter/CMakeLists.txt b/planning/autoware_planning_topic_converter/CMakeLists.txt similarity index 83% rename from planning/planning_topic_converter/CMakeLists.txt rename to planning/autoware_planning_topic_converter/CMakeLists.txt index 75b63d26c316f..ae1b585895e5e 100644 --- a/planning/planning_topic_converter/CMakeLists.txt +++ b/planning/autoware_planning_topic_converter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(planning_topic_converter) +project(autoware_planning_topic_converter) ### Compile options if(NOT CMAKE_CXX_STANDARD) @@ -17,7 +17,7 @@ ament_auto_add_library(planning_topic_converter SHARED ) rclcpp_components_register_node(planning_topic_converter - PLUGIN "planning_topic_converter::PathToTrajectory" + PLUGIN "autoware::planning_topic_converter::PathToTrajectory" EXECUTABLE path_to_trajectory_converter ) diff --git a/planning/planning_topic_converter/README.md b/planning/autoware_planning_topic_converter/README.md similarity index 86% rename from planning/planning_topic_converter/README.md rename to planning/autoware_planning_topic_converter/README.md index 74137cda0197d..e5036c6c0903c 100644 --- a/planning/planning_topic_converter/README.md +++ b/planning/autoware_planning_topic_converter/README.md @@ -12,7 +12,7 @@ The tools in this package are provided as composable ROS 2 component nodes, so t ```xml - + diff --git a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp similarity index 83% rename from planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp index a093d9a952029..e3b0fe5069e92 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/converter_base.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/converter_base.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ -#define PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ #include "rclcpp/rclcpp.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { template @@ -46,6 +46,6 @@ class ConverterBase : public rclcpp::Node private: }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__CONVERTER_BASE_HPP_ diff --git a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp similarity index 75% rename from planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp rename to planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp index 708f624e97898..7ca50d6c4e50c 100644 --- a/planning/planning_topic_converter/include/planning_topic_converter/path_to_trajectory.hpp +++ b/planning/autoware_planning_topic_converter/include/autoware_planning_topic_converter/path_to_trajectory.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#define PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#ifndef AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#define AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ -#include "planning_topic_converter/converter_base.hpp" +#include "autoware_planning_topic_converter/converter_base.hpp" #include "rclcpp/rclcpp.hpp" #include @@ -23,7 +23,7 @@ #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { using autoware_planning_msgs::msg::Path; @@ -40,6 +40,6 @@ class PathToTrajectory : public ConverterBase void process(const Path::ConstSharedPtr msg) override; }; -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter -#endif // PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ +#endif // AUTOWARE_PLANNING_TOPIC_CONVERTER__PATH_TO_TRAJECTORY_HPP_ diff --git a/planning/planning_topic_converter/package.xml b/planning/autoware_planning_topic_converter/package.xml similarity index 86% rename from planning/planning_topic_converter/package.xml rename to planning/autoware_planning_topic_converter/package.xml index 3699f3e53900c..be44523fa9852 100644 --- a/planning/planning_topic_converter/package.xml +++ b/planning/autoware_planning_topic_converter/package.xml @@ -1,8 +1,8 @@ - planning_topic_converter + autoware_planning_topic_converter 0.1.0 - The planning_topic_converter package + The autoware_planning_topic_converter package Satoshi OTA Shumpei Wakabayashi diff --git a/planning/planning_topic_converter/src/path_to_trajectory.cpp b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp similarity index 88% rename from planning/planning_topic_converter/src/path_to_trajectory.cpp rename to planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp index 757dcd0d9f632..d7aceb1abd106 100644 --- a/planning/planning_topic_converter/src/path_to_trajectory.cpp +++ b/planning/autoware_planning_topic_converter/src/path_to_trajectory.cpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_topic_converter/path_to_trajectory.hpp" +#include "autoware_planning_topic_converter/path_to_trajectory.hpp" #include #include -namespace planning_topic_converter +namespace autoware::planning_topic_converter { namespace { @@ -54,7 +54,7 @@ void PathToTrajectory::process(const Path::ConstSharedPtr msg) pub_->publish(output); } -} // namespace planning_topic_converter +} // namespace autoware::planning_topic_converter #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_topic_converter::PathToTrajectory) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_topic_converter::PathToTrajectory) diff --git a/planning/behavior_velocity_run_out_module/plugins.xml b/planning/behavior_velocity_run_out_module/plugins.xml deleted file mode 100644 index 5320d46f92929..0000000000000 --- a/planning/behavior_velocity_run_out_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index d9b5d4615e148..a1ef2c408ce3b 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -5,6 +5,7 @@ 0.1.0 The surround_obstacle_checker package Satoshi Ota + Go Sakayori Apache License 2.0 Satoshi Ota diff --git a/sensing/vehicle_velocity_converter/CMakeLists.txt b/sensing/vehicle_velocity_converter/CMakeLists.txt index 4f4a14bd9f045..bb50fbff90c4b 100644 --- a/sensing/vehicle_velocity_converter/CMakeLists.txt +++ b/sensing/vehicle_velocity_converter/CMakeLists.txt @@ -4,11 +4,15 @@ project(vehicle_velocity_converter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(vehicle_velocity_converter - src/vehicle_velocity_converter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/vehicle_velocity_converter.cpp ) -ament_target_dependencies(vehicle_velocity_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "VehicleVelocityConverter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp index 4a1a66b842892..8ef46609ee542 100644 --- a/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp +++ b/sensing/vehicle_velocity_converter/include/vehicle_velocity_converter/vehicle_velocity_converter.hpp @@ -28,7 +28,7 @@ class VehicleVelocityConverter : public rclcpp::Node { public: - VehicleVelocityConverter(); + explicit VehicleVelocityConverter(const rclcpp::NodeOptions & options); ~VehicleVelocityConverter() = default; private: diff --git a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml index 6302c09b526f9..84e1838dc89eb 100644 --- a/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml +++ b/sensing/vehicle_velocity_converter/launch/vehicle_velocity_converter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/sensing/vehicle_velocity_converter/package.xml b/sensing/vehicle_velocity_converter/package.xml index 39780deaccc28..b32b51bc52e10 100644 --- a/sensing/vehicle_velocity_converter/package.xml +++ b/sensing/vehicle_velocity_converter/package.xml @@ -13,6 +13,7 @@ autoware_vehicle_msgs geometry_msgs rclcpp + rclcpp_components ament_lint_auto autoware_lint_common diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp index 360ec4cae58d5..2aece69f5283b 100644 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp +++ b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter.cpp @@ -14,7 +14,8 @@ #include "vehicle_velocity_converter/vehicle_velocity_converter.hpp" -VehicleVelocityConverter::VehicleVelocityConverter() : Node("vehicle_velocity_converter") +VehicleVelocityConverter::VehicleVelocityConverter(const rclcpp::NodeOptions & options) +: rclcpp::Node("vehicle_velocity_converter", options) { // set covariance value for twist with covariance msg stddev_vx_ = declare_parameter("velocity_stddev_xx"); @@ -52,3 +53,6 @@ void VehicleVelocityConverter::callbackVelocityReport( twist_with_covariance_pub_->publish(twist_with_covariance_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(VehicleVelocityConverter) diff --git a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp b/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp deleted file mode 100644 index cee25f6e6c62a..0000000000000 --- a/sensing/vehicle_velocity_converter/src/vehicle_velocity_converter_node.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2021 TierIV -// -// 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 - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp index 5ff269705456d..012bb55af3068 100644 --- a/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp +++ b/system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp @@ -130,11 +130,13 @@ class EmergencyHandler : public rclcpp::Node void checkHazardStatusTimeout(); // Algorithm + uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); + bool isDrivingBackwards(); bool isEmergency(); }; diff --git a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp index b96cb0d0549f9..ae3d60e32a445 100644 --- a/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp +++ b/system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp @@ -164,12 +164,17 @@ void EmergencyHandler::publishControlCommands() { GearCommand msg; msg.stamp = stamp; - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } + const auto command = [&]() { + // If stopped and use_parking is not true, send the last gear command + if (isStopped()) + return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; + return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; + }(); + + msg.command = command; + last_gear_command_ = msg.command; pub_gear_cmd_->publish(msg); + return; } } @@ -451,11 +456,13 @@ bool EmergencyHandler::isEmergency() bool EmergencyHandler::isStopped() { constexpr auto th_stopped_velocity = 0.001; - if (odom_->twist.twist.linear.x < th_stopped_velocity) { - return true; - } + return (std::abs(odom_->twist.twist.linear.x) < th_stopped_velocity); +} - return false; +bool EmergencyHandler::isDrivingBackwards() +{ + constexpr auto th_moving_backwards = -0.001; + return odom_->twist.twist.linear.x < th_moving_backwards; } #include diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 8c58d961953ce..7a160b6c531e9 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -147,12 +147,14 @@ class MrmHandler : public rclcpp::Node // Algorithm bool is_emergency_holding_ = false; + uint8_t last_gear_command_{autoware_vehicle_msgs::msg::GearCommand::DRIVE}; void transitionTo(const int new_state); void updateMrmState(); void operateMrm(); void handleFailedRequest(); autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); bool isStopped(); + bool isDrivingBackwards(); bool isEmergency() const; bool isArrivedAtGoal(); }; diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index afcc7ebd208ab..d70303a9801ac 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -191,13 +191,17 @@ void MrmHandler::publishGearCmd() GearCommand msg; msg.stamp = this->now(); - if (param_.use_parking_after_stopped && isStopped()) { - msg.command = GearCommand::PARK; - } else { - msg.command = GearCommand::DRIVE; - } - + const auto command = [&]() { + // If stopped and use_parking is not true, send the last gear command + if (isStopped()) + return (param_.use_parking_after_stopped) ? GearCommand::PARK : last_gear_command_; + return (isDrivingBackwards()) ? GearCommand::REVERSE : GearCommand::DRIVE; + }(); + + msg.command = command; + last_gear_command_ = msg.command; pub_gear_cmd_->publish(msg); + return; } void MrmHandler::publishMrmState() @@ -449,48 +453,48 @@ void MrmHandler::updateMrmState() // Check emergency const bool is_emergency = isEmergency(); + // Send recovery events if is not an emergency + if (!is_emergency) { + if (mrm_state_.state != MrmState::NORMAL) transitionTo(MrmState::NORMAL); + return; + } + // Get mode const bool is_auto_mode = control_mode_->mode == ControlModeReport::AUTONOMOUS; // State Machine - if (mrm_state_.state == MrmState::NORMAL) { - // NORMAL - if (is_auto_mode && is_emergency) { - transitionTo(MrmState::MRM_OPERATING); - return; - } - } else { - // Emergency - // Send recovery events if "not emergency" - if (!is_emergency) { - transitionTo(MrmState::NORMAL); + switch (mrm_state_.state) { + case MrmState::NORMAL: + if (is_auto_mode) { + transitionTo(MrmState::MRM_OPERATING); + } return; - } - if (mrm_state_.state == MrmState::MRM_OPERATING) { - // TODO(TetsuKawa): Check MRC is accomplished - if (mrm_state_.behavior == MrmState::PULL_OVER) { - if (isStopped() && isArrivedAtGoal()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } - } else { - if (isStopped()) { - transitionTo(MrmState::MRM_SUCCEEDED); - return; - } + case MrmState::MRM_OPERATING: + if (!isStopped()) return; + if (mrm_state_.behavior != MrmState::PULL_OVER) { + transitionTo(MrmState::MRM_SUCCEEDED); + return; } - } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { - const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { + if (isArrivedAtGoal()) { + transitionTo(MrmState::MRM_SUCCEEDED); + } + return; + + case MrmState::MRM_SUCCEEDED: + if (mrm_state_.behavior != getCurrentMrmBehavior()) { transitionTo(MrmState::MRM_OPERATING); } - } else if (mrm_state_.state == MrmState::MRM_FAILED) { + return; + case MrmState::MRM_FAILED: // Do nothing(only checking common recovery events) - } else { + return; + + default: { const auto msg = "invalid state: " + std::to_string(mrm_state_.state); throw std::runtime_error(msg); } + return; } } @@ -578,11 +582,13 @@ autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmB bool MrmHandler::isStopped() { constexpr auto th_stopped_velocity = 0.001; - if (odom_->twist.twist.linear.x < th_stopped_velocity) { - return true; - } + return std::abs((odom_->twist.twist.linear.x < th_stopped_velocity) < th_stopped_velocity); +} - return false; +bool MrmHandler::isDrivingBackwards() +{ + constexpr auto th_moving_backwards = -0.001; + return odom_->twist.twist.linear.x < th_moving_backwards; } bool MrmHandler::isEmergency() const