diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 029f6cdb5ca95..2eb1bdcdfcb33 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -156,6 +156,7 @@ perception/traffic_light_map_based_detector/** shunsuke.miura@tier4.jp tao.zhong perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_visualization/** tao.zhong@tier4.jp yukihiro.saito@tier4.jp +planning/autoware_behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/autoware_behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp planning/autoware_remaining_distance_time_calculator/** ahmed.ebrahim@leodrive.ai @@ -163,7 +164,6 @@ planning/autoware_static_centerline_generator/** kosuke.takeuchi@tier4.jp takayu planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp -planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** daniel.sanchez@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_planner/** fumiya.watanabe@tier4.jp go.sakayori@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp index 926fbb7435f3a..eaa07f2317940 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics/metric.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include namespace perception_diagnostics @@ -36,7 +37,11 @@ enum class Metric { SIZE, }; +// Each metric has a different return type. (statistic or just a one value etc). +// To handle them all in the MetricsCalculator::calculate function, define MetricsMap as a variant using MetricStatMap = std::unordered_map>; +using MetricValueMap = std::unordered_map; +using MetricsMap = std::variant; struct PredictedPathDeviationMetrics { diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp index 96b1c01ee2d16..a04819d4844ce 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/metrics_calculator.hpp @@ -96,7 +96,7 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return map of string describing the requested metric and the calculated value */ - std::optional calculate(const Metric & metric) const; + std::optional calculate(const Metric & metric) const; /** * @brief set the dynamic objects used to calculate obstacle metrics @@ -143,7 +143,7 @@ class MetricsCalculator PredictedPathDeviationMetrics calcPredictedPathDeviationMetrics( const PredictedObjects & objects, const double time_horizon) const; MetricStatMap calcYawRateMetrics(const ClassObjectsMap & class_objects_map) const; - MetricStatMap calcObjectsCountMetrics() const; + MetricValueMap calcObjectsCountMetrics() const; bool hasPassedTime(const rclcpp::Time stamp) const; bool hasPassedTime(const std::string uuid, const rclcpp::Time stamp) const; diff --git a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp index e5c41ff28c4da..070d938b8bd0a 100644 --- a/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp +++ b/evaluator/perception_online_evaluator/include/perception_online_evaluator/perception_online_evaluator_node.hpp @@ -61,6 +61,8 @@ class PerceptionOnlineEvaluatorNode : public rclcpp::Node DiagnosticStatus generateDiagnosticStatus( const std::string metric, const Stat & metric_stat) const; + DiagnosticStatus generateDiagnosticStatus( + const std::string metric, const double metric_value) const; private: // Subscribers and publishers diff --git a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml index 807f247807d98..16d4b6fe458cd 100644 --- a/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml +++ b/evaluator/perception_online_evaluator/param/perception_online_evaluator.defaults.yaml @@ -13,7 +13,7 @@ prediction_time_horizons: [1.0, 2.0, 3.0, 5.0] stopped_velocity_threshold: 1.0 - detection_radius_list: [50.0, 100.0, 150.0, 200.0] + detection_radius_list: [20.0, 40.0, 60.0, 80.0, 100.0, 120.0, 140.0, 160.0, 180.0, 200.0] detection_height_list: [10.0] detection_count_purge_seconds: 36000.0 objects_count_window_seconds: 1.0 diff --git a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp index 9c1e0667e4fef..cc455445ca9f8 100644 --- a/evaluator/perception_online_evaluator/src/metrics_calculator.cpp +++ b/evaluator/perception_online_evaluator/src/metrics_calculator.cpp @@ -26,7 +26,7 @@ namespace perception_diagnostics using object_recognition_utils::convertLabelToString; using tier4_autoware_utils::inverseTransformPoint; -std::optional MetricsCalculator::calculate(const Metric & metric) const +std::optional MetricsCalculator::calculate(const Metric & metric) const { // clang-format off const bool use_past_objects = metric == Metric::lateral_deviation || @@ -455,15 +455,14 @@ MetricStatMap MetricsCalculator::calcYawRateMetrics(const ClassObjectsMap & clas return metric_stat_map; } -MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const +MetricValueMap MetricsCalculator::calcObjectsCountMetrics() const { - MetricStatMap metric_stat_map; + MetricValueMap metric_stat_map; // calculate the average number of objects in the detection area in all past frames const auto overall_average_count = detection_counter_.getOverallAverageCount(); for (const auto & [label, range_and_count] : overall_average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } // calculate the average number of objects in the detection area in the past @@ -472,8 +471,8 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const detection_counter_.getAverageCount(parameters_->objects_count_window_seconds); for (const auto & [label, range_and_count] : average_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] - .add(count); + metric_stat_map + ["interval_average_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } @@ -481,8 +480,7 @@ MetricStatMap MetricsCalculator::calcObjectsCountMetrics() const const auto total_count = detection_counter_.getTotalCount(); for (const auto & [label, range_and_count] : total_count) { for (const auto & [range, count] : range_and_count) { - metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range].add( - count); + metric_stat_map["total_objects_count_" + convertLabelToString(label) + "_" + range] = count; } } diff --git a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp index 0fcdd77f4d515..be9ac2332f62c 100644 --- a/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/src/perception_online_evaluator_node.cpp @@ -69,16 +69,25 @@ void PerceptionOnlineEvaluatorNode::publishMetrics() // calculate metrics for (const Metric & metric : parameters_->metrics) { - const auto metric_stat_map = metrics_calculator_.calculate(Metric(metric)); - if (!metric_stat_map.has_value()) { + const auto metric_result = metrics_calculator_.calculate(Metric(metric)); + if (!metric_result.has_value()) { continue; } - for (const auto & [metric, stat] : metric_stat_map.value()) { - if (stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, stat)); - } - } + std::visit( + [&metrics_msg, this](auto && arg) { + using T = std::decay_t; + for (const auto & [metric, value] : arg) { + if constexpr (std::is_same_v) { + if (value.count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } else if constexpr (std::is_same_v) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, value)); + } + } + }, + metric_result.value()); } // publish metrics @@ -111,6 +120,22 @@ DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( return status; } +DiagnosticStatus PerceptionOnlineEvaluatorNode::generateDiagnosticStatus( + const std::string metric, const double value) const +{ + DiagnosticStatus status; + + status.level = status.OK; + status.name = metric; + + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = "metric_value"; + key_value.value = std::to_string(value); + status.values.push_back(key_value); + + return status; +} + void PerceptionOnlineEvaluatorNode::onObjects(const PredictedObjects::ConstSharedPtr objects_msg) { metrics_calculator_.setPredictedObjects(*objects_msg, *tf_buffer_); diff --git a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp index 95d6f07cca5d9..53ef0fb99c8e3 100644 --- a/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp +++ b/evaluator/perception_online_evaluator/test/test_perception_online_evaluator_node.cpp @@ -141,7 +141,19 @@ class EvalTest : public ::testing::Test [=](const DiagnosticArray::ConstSharedPtr msg) { const auto it = std::find_if(msg->status.begin(), msg->status.end(), is_target_metric); if (it != msg->status.end()) { - metric_value_ = boost::lexical_cast(it->values[2].value); + const auto mean_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "mean"; }); + if (mean_it != it->values.end()) { + metric_value_ = boost::lexical_cast(mean_it->value); + } else { + const auto metric_value_it = std::find_if( + it->values.begin(), it->values.end(), + [](const auto & key_value) { return key_value.key == "metric_value"; }); + if (metric_value_it != it->values.end()) { + metric_value_ = boost::lexical_cast(metric_value_it->value); + } + } metric_updated_ = true; } }); 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 adfe7549300c4..57d0280330641 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 @@ -82,12 +82,12 @@ /> warning_; diff --git a/localization/ekf_localizer/launch/ekf_localizer.launch.xml b/localization/ekf_localizer/launch/ekf_localizer.launch.xml index b6a1bd06185c2..3c66fabe34650 100644 --- a/localization/ekf_localizer/launch/ekf_localizer.launch.xml +++ b/localization/ekf_localizer/launch/ekf_localizer.launch.xml @@ -17,7 +17,7 @@ - + diff --git a/localization/ekf_localizer/package.xml b/localization/ekf_localizer/package.xml index e9d756e547f26..dad1b0e730711 100644 --- a/localization/ekf_localizer/package.xml +++ b/localization/ekf_localizer/package.xml @@ -29,6 +29,7 @@ kalman_filter nav_msgs rclcpp + rclcpp_components std_srvs tf2 tf2_ros diff --git a/localization/ekf_localizer/src/ekf_localizer.cpp b/localization/ekf_localizer/src/ekf_localizer.cpp index f77481d45a534..687e812679574 100644 --- a/localization/ekf_localizer/src/ekf_localizer.cpp +++ b/localization/ekf_localizer/src/ekf_localizer.cpp @@ -40,8 +40,8 @@ using std::placeholders::_1; -EKFLocalizer::EKFLocalizer(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options), +EKFLocalizer::EKFLocalizer(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("ekf_localizer", node_options), warning_(std::make_shared(this)), params_(this), ekf_dt_(params_.ekf_dt), @@ -479,3 +479,6 @@ void EKFLocalizer::serviceTriggerNode( res->success = true; return; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(EKFLocalizer) diff --git a/localization/ekf_localizer/src/ekf_localizer_node.cpp b/localization/ekf_localizer/src/ekf_localizer_node.cpp deleted file mode 100644 index 0f75f19c5d50b..0000000000000 --- a/localization/ekf_localizer/src/ekf_localizer_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2018-2019 Autoware Foundation -// -// 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 "ekf_localizer/ekf_localizer.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("ekf_localizer", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt index 57589837dd529..a832383ff4761 100644 --- a/localization/gyro_odometer/CMakeLists.txt +++ b/localization/gyro_odometer/CMakeLists.txt @@ -4,17 +4,12 @@ project(gyro_odometer) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(${PROJECT_NAME} - src/gyro_odometer_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/gyro_odometer_core.cpp ) target_link_libraries(${PROJECT_NAME} fmt) -ament_auto_add_library(gyro_odometer_node SHARED - src/gyro_odometer_core.cpp -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_gyro_odometer test/test_main.cpp @@ -25,10 +20,15 @@ if(BUILD_TESTING) rclcpp ) target_link_libraries(test_gyro_odometer - gyro_odometer_node + ${PROJECT_NAME} ) endif() +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "autoware::gyro_odometer::GyroOdometerNode" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package(INSTALL_TO_SHARE launch diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp index 3a6358e62c21a..2926589bd2da9 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -36,14 +36,17 @@ #include #include -class GyroOdometer : public rclcpp::Node +namespace autoware::gyro_odometer +{ + +class GyroOdometerNode : public rclcpp::Node { private: using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - explicit GyroOdometer(const rclcpp::NodeOptions & options); - ~GyroOdometer(); + explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); + ~GyroOdometerNode(); private: void callbackVehicleTwist( @@ -75,4 +78,6 @@ class GyroOdometer : public rclcpp::Node std::deque gyro_queue_; }; +} // namespace autoware::gyro_odometer + #endif // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_ diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml index 21aff3e10d26c..aed6050858fe1 100644 --- a/localization/gyro_odometer/launch/gyro_odometer.launch.xml +++ b/localization/gyro_odometer/launch/gyro_odometer.launch.xml @@ -11,7 +11,7 @@ - + diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml index a0a982ddedc16..3c979eb69ac8a 100644 --- a/localization/gyro_odometer/package.xml +++ b/localization/gyro_odometer/package.xml @@ -19,14 +19,13 @@ fmt geometry_msgs + rclcpp + rclcpp_components sensor_msgs tf2 tf2_geometry_msgs tier4_autoware_utils - rclcpp - rclcpp_components - ament_cmake_ros ament_lint_auto autoware_lint_common diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 5de0ecd7cdc0a..bda7af74b8489 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -14,6 +14,8 @@ #include "gyro_odometer/gyro_odometer_core.hpp" +#include + #ifdef ROS_DISTRO_GALACTIC #include #else @@ -25,6 +27,42 @@ #include #include +namespace autoware::gyro_odometer +{ + +GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) +: Node("gyro_odometer", node_options), + output_frame_(declare_parameter("output_frame")), + message_timeout_sec_(declare_parameter("message_timeout_sec")), + vehicle_twist_arrived_(false), + imu_arrived_(false) +{ + transform_listener_ = std::make_shared(this); + logger_configure_ = std::make_unique(this); + + vehicle_twist_sub_ = create_subscription( + "vehicle/twist_with_covariance", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + + imu_sub_ = create_subscription( + "imu", rclcpp::QoS{100}, + std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + + twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); + twist_with_covariance_raw_pub_ = create_publisher( + "twist_with_covariance_raw", rclcpp::QoS{10}); + + twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); + twist_with_covariance_pub_ = create_publisher( + "twist_with_covariance", rclcpp::QoS{10}); + + // TODO(YamatoAndo) createTimer +} + +GyroOdometerNode::~GyroOdometerNode() +{ +} + std::array transformCovariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -100,39 +138,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer( return twist_with_cov; } -GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options) -: Node("gyro_odometer", options), - output_frame_(declare_parameter("output_frame")), - message_timeout_sec_(declare_parameter("message_timeout_sec")), - vehicle_twist_arrived_(false), - imu_arrived_(false) -{ - transform_listener_ = std::make_shared(this); - logger_configure_ = std::make_unique(this); - - vehicle_twist_sub_ = create_subscription( - "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometer::callbackVehicleTwist, this, std::placeholders::_1)); - - imu_sub_ = create_subscription( - "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1)); - - twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); - twist_with_covariance_raw_pub_ = create_publisher( - "twist_with_covariance_raw", rclcpp::QoS{10}); - - twist_pub_ = create_publisher("twist", rclcpp::QoS{10}); - twist_with_covariance_pub_ = create_publisher( - "twist_with_covariance", rclcpp::QoS{10}); - - // TODO(YamatoAndo) createTimer -} - -GyroOdometer::~GyroOdometer() -{ -} - -void GyroOdometer::callbackVehicleTwist( +void GyroOdometerNode::callbackVehicleTwist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) { vehicle_twist_arrived_ = true; @@ -173,7 +179,7 @@ void GyroOdometer::callbackVehicleTwist( gyro_queue_.clear(); } -void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { imu_arrived_ = true; if (!vehicle_twist_arrived_) { @@ -241,7 +247,7 @@ void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m gyro_queue_.clear(); } -void GyroOdometer::publishData( +void GyroOdometerNode::publishData( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; @@ -269,3 +275,8 @@ void GyroOdometer::publishData( twist_pub_->publish(twist); twist_with_covariance_pub_->publish(twist_with_covariance); } + +} // namespace autoware::gyro_odometer + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::gyro_odometer::GyroOdometerNode) diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp deleted file mode 100644 index 5bb6241506fbe..0000000000000 --- a/localization/gyro_odometer/src/gyro_odometer_node.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "gyro_odometer/gyro_odometer_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - auto node = std::make_shared(options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index 7f6416fbdda16..b7849ef03bfc5 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -108,7 +108,8 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) expected_output_twist.twist.twist.angular.y = input_imu.angular_velocity.y; expected_output_twist.twist.twist.angular.z = input_imu.angular_velocity.z; - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -135,7 +136,8 @@ TEST(GyroOdometer, TestGyroOdometerImuOnly) { Imu input_imu = generateSampleImu(); - auto gyro_odometer_node = std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = + std::make_shared(getNodeOptionsWithDefaultParams()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); diff --git a/localization/localization_error_monitor/CMakeLists.txt b/localization/localization_error_monitor/CMakeLists.txt index 3528367486350..c27e51e6e0359 100644 --- a/localization/localization_error_monitor/CMakeLists.txt +++ b/localization/localization_error_monitor/CMakeLists.txt @@ -4,15 +4,16 @@ project(localization_error_monitor) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(localization_error_monitor_module SHARED +ament_auto_add_library(${PROJECT_NAME} SHARED src/diagnostics.cpp + src/localization_error_monitor.cpp ) -ament_auto_add_executable(localization_error_monitor - src/main.cpp - src/node.cpp +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "LocalizationErrorMonitor" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor ) -target_link_libraries(localization_error_monitor localization_error_monitor_module) if(BUILD_TESTING) function(add_testcase filepath) @@ -20,17 +21,17 @@ if(BUILD_TESTING) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gtest(${test_name} ${filepath}) target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} localization_error_monitor_module) + target_link_libraries(${test_name} ${PROJECT_NAME}) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() - find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() add_testcase(test/test_diagnostics.cpp) endif() -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE config launch ) diff --git a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp similarity index 87% rename from localization/localization_error_monitor/include/localization_error_monitor/node.hpp rename to localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp index 296b278004333..6016677d88136 100644 --- a/localization/localization_error_monitor/include/localization_error_monitor/node.hpp +++ b/localization/localization_error_monitor/include/localization_error_monitor/localization_error_monitor.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_ERROR_MONITOR__NODE_HPP_ -#define LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#ifndef LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ +#define LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ #include #include @@ -58,7 +58,7 @@ class LocalizationErrorMonitor : public rclcpp::Node double measureSizeEllipseAlongBodyFrame(const Eigen::Matrix2d & Pinv, double theta); public: - LocalizationErrorMonitor(); + explicit LocalizationErrorMonitor(const rclcpp::NodeOptions & options); ~LocalizationErrorMonitor() = default; }; -#endif // LOCALIZATION_ERROR_MONITOR__NODE_HPP_ +#endif // LOCALIZATION_ERROR_MONITOR__LOCALIZATION_ERROR_MONITOR_HPP_ diff --git a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml index eb2b5741b00fc..ad3e8beff92ab 100644 --- a/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml +++ b/localization/localization_error_monitor/launch/localization_error_monitor.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/localization_error_monitor/package.xml b/localization/localization_error_monitor/package.xml index a1b352d911a0d..0138451b069e4 100644 --- a/localization/localization_error_monitor/package.xml +++ b/localization/localization_error_monitor/package.xml @@ -23,6 +23,7 @@ diagnostic_msgs diagnostic_updater nav_msgs + rclcpp_components tf2 tf2_geometry_msgs tier4_autoware_utils diff --git a/localization/localization_error_monitor/src/node.cpp b/localization/localization_error_monitor/src/localization_error_monitor.cpp similarity index 94% rename from localization/localization_error_monitor/src/node.cpp rename to localization/localization_error_monitor/src/localization_error_monitor.cpp index f47372a0158b2..67cdb78c942fb 100644 --- a/localization/localization_error_monitor/src/node.cpp +++ b/localization/localization_error_monitor/src/localization_error_monitor.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "localization_error_monitor/node.hpp" +#include "localization_error_monitor/localization_error_monitor.hpp" #include "localization_error_monitor/diagnostics.hpp" @@ -32,7 +32,8 @@ #include #include -LocalizationErrorMonitor::LocalizationErrorMonitor() : Node("localization_error_monitor") +LocalizationErrorMonitor::LocalizationErrorMonitor(const rclcpp::NodeOptions & options) +: Node("localization_error_monitor", options) { scale_ = this->declare_parameter("scale"); error_ellipse_size_ = this->declare_parameter("error_ellipse_size"); @@ -143,3 +144,6 @@ double LocalizationErrorMonitor::measureSizeEllipseAlongBodyFrame( double d = std::sqrt((e.transpose() * Pinv * e)(0, 0) / Pinv.determinant()); return d; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(LocalizationErrorMonitor) diff --git a/localization/localization_error_monitor/src/main.cpp b/localization/localization_error_monitor/src/main.cpp deleted file mode 100644 index f4fa5ab664005..0000000000000 --- a/localization/localization_error_monitor/src/main.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2020 Tier IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_error_monitor/node.hpp" - -#include - -#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/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt index a2fbbddf7d120..2a586aa9cd049 100644 --- a/localization/pose2twist/CMakeLists.txt +++ b/localization/pose2twist/CMakeLists.txt @@ -4,11 +4,16 @@ project(pose2twist) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(pose2twist - src/pose2twist_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/pose2twist_core.cpp ) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Pose2Twist" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + ament_auto_package( INSTALL_TO_SHARE launch diff --git a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp index c2a39f7e2ed3d..459a62ea5cd13 100644 --- a/localization/pose2twist/include/pose2twist/pose2twist_core.hpp +++ b/localization/pose2twist/include/pose2twist/pose2twist_core.hpp @@ -24,7 +24,7 @@ class Pose2Twist : public rclcpp::Node { public: - Pose2Twist(); + explicit Pose2Twist(const rclcpp::NodeOptions & options); ~Pose2Twist() = default; private: diff --git a/localization/pose2twist/launch/pose2twist.launch.xml b/localization/pose2twist/launch/pose2twist.launch.xml index a0fae57a163e6..57a41dbfcf017 100644 --- a/localization/pose2twist/launch/pose2twist.launch.xml +++ b/localization/pose2twist/launch/pose2twist.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/localization/pose2twist/package.xml b/localization/pose2twist/package.xml index 16c49bb51c218..07e445c72978c 100644 --- a/localization/pose2twist/package.xml +++ b/localization/pose2twist/package.xml @@ -19,6 +19,7 @@ geometry_msgs rclcpp + rclcpp_components tf2_geometry_msgs tier4_debug_msgs diff --git a/localization/pose2twist/src/pose2twist_core.cpp b/localization/pose2twist/src/pose2twist_core.cpp index a321a06122816..4b98ec6c81ad4 100644 --- a/localization/pose2twist/src/pose2twist_core.cpp +++ b/localization/pose2twist/src/pose2twist_core.cpp @@ -24,7 +24,7 @@ #include #include -Pose2Twist::Pose2Twist() : Node("pose2twist_core") +Pose2Twist::Pose2Twist(const rclcpp::NodeOptions & options) : rclcpp::Node("pose2twist", options) { using std::placeholders::_1; @@ -129,3 +129,6 @@ void Pose2Twist::callbackPose(geometry_msgs::msg::PoseStamped::SharedPtr pose_ms angular_z_msg.data = twist_msg.twist.angular.z; angular_z_pub_->publish(angular_z_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Pose2Twist) diff --git a/localization/pose2twist/src/pose2twist_node.cpp b/localization/pose2twist/src/pose2twist_node.cpp deleted file mode 100644 index 81f98461ecffd..0000000000000 --- a/localization/pose2twist/src/pose2twist_node.cpp +++ /dev/null @@ -1,29 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// 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 "pose2twist/pose2twist_core.hpp" - -#include - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} diff --git a/localization/stop_filter/CMakeLists.txt b/localization/stop_filter/CMakeLists.txt index 97a0443195ae5..4776a1f4967b2 100644 --- a/localization/stop_filter/CMakeLists.txt +++ b/localization/stop_filter/CMakeLists.txt @@ -4,11 +4,15 @@ project(stop_filter) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(stop_filter - src/stop_filter_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/stop_filter.cpp ) -ament_target_dependencies(stop_filter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "StopFilter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/stop_filter/include/stop_filter/stop_filter.hpp b/localization/stop_filter/include/stop_filter/stop_filter.hpp index 3c2b91590234f..24145a7920d91 100644 --- a/localization/stop_filter/include/stop_filter/stop_filter.hpp +++ b/localization/stop_filter/include/stop_filter/stop_filter.hpp @@ -37,7 +37,7 @@ class StopFilter : public rclcpp::Node { public: - StopFilter(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit StopFilter(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr pub_odom_; //!< @brief odom publisher diff --git a/localization/stop_filter/launch/stop_filter.launch.xml b/localization/stop_filter/launch/stop_filter.launch.xml index 0ea92d26c9677..c53b37efc9954 100644 --- a/localization/stop_filter/launch/stop_filter.launch.xml +++ b/localization/stop_filter/launch/stop_filter.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/localization/stop_filter/package.xml b/localization/stop_filter/package.xml index 83eaf1b9fa821..2e6d5c4f8bbe1 100644 --- a/localization/stop_filter/package.xml +++ b/localization/stop_filter/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components tf2 tier4_debug_msgs diff --git a/localization/stop_filter/src/stop_filter.cpp b/localization/stop_filter/src/stop_filter.cpp index ac0960b8cb67b..4d6b2c6240867 100644 --- a/localization/stop_filter/src/stop_filter.cpp +++ b/localization/stop_filter/src/stop_filter.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -StopFilter::StopFilter(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +StopFilter::StopFilter(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("stop_filter", node_options) { vx_threshold_ = declare_parameter("vx_threshold"); wz_threshold_ = declare_parameter("wz_threshold"); @@ -57,3 +57,6 @@ void StopFilter::callbackOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) pub_stop_flag_->publish(stop_flag_msg); pub_odom_->publish(odom_msg); } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter) diff --git a/localization/stop_filter/src/stop_filter_node.cpp b/localization/stop_filter/src/stop_filter_node.cpp deleted file mode 100644 index 9748214828de2..0000000000000 --- a/localization/stop_filter/src/stop_filter_node.cpp +++ /dev/null @@ -1,28 +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 "stop_filter/stop_filter.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("stop_filter", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/localization/twist2accel/CMakeLists.txt b/localization/twist2accel/CMakeLists.txt index 59f23eacb2fb3..9178bf02288a8 100644 --- a/localization/twist2accel/CMakeLists.txt +++ b/localization/twist2accel/CMakeLists.txt @@ -4,11 +4,15 @@ project(twist2accel) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(twist2accel - src/twist2accel_node.cpp +ament_auto_add_library(${PROJECT_NAME} SHARED src/twist2accel.cpp ) -ament_target_dependencies(twist2accel) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "Twist2Accel" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) ament_auto_package( INSTALL_TO_SHARE diff --git a/localization/twist2accel/include/twist2accel/twist2accel.hpp b/localization/twist2accel/include/twist2accel/twist2accel.hpp index da5f2b4a3b228..0db69890fbfe8 100644 --- a/localization/twist2accel/include/twist2accel/twist2accel.hpp +++ b/localization/twist2accel/include/twist2accel/twist2accel.hpp @@ -40,7 +40,7 @@ class Twist2Accel : public rclcpp::Node { public: - Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & options); + explicit Twist2Accel(const rclcpp::NodeOptions & options); private: rclcpp::Publisher::SharedPtr diff --git a/localization/twist2accel/launch/twist2accel.launch.xml b/localization/twist2accel/launch/twist2accel.launch.xml index c4c9a3ed50bfc..c534a288aac3e 100644 --- a/localization/twist2accel/launch/twist2accel.launch.xml +++ b/localization/twist2accel/launch/twist2accel.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/localization/twist2accel/package.xml b/localization/twist2accel/package.xml index 08dacf9185769..0dbce08f309ac 100644 --- a/localization/twist2accel/package.xml +++ b/localization/twist2accel/package.xml @@ -21,6 +21,7 @@ geometry_msgs nav_msgs rclcpp + rclcpp_components signal_processing tf2 tier4_debug_msgs diff --git a/localization/twist2accel/src/twist2accel.cpp b/localization/twist2accel/src/twist2accel.cpp index 68019f27e95fe..0af29445bbeb8 100644 --- a/localization/twist2accel/src/twist2accel.cpp +++ b/localization/twist2accel/src/twist2accel.cpp @@ -24,8 +24,8 @@ using std::placeholders::_1; -Twist2Accel::Twist2Accel(const std::string & node_name, const rclcpp::NodeOptions & node_options) -: rclcpp::Node(node_name, node_options) +Twist2Accel::Twist2Accel(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("twist2accel", node_options) { sub_odom_ = create_subscription( "input/odom", 1, std::bind(&Twist2Accel::callbackOdometry, this, _1)); @@ -103,3 +103,6 @@ void Twist2Accel::estimateAccel(const geometry_msgs::msg::TwistStamped::SharedPt pub_accel_->publish(accel_msg); prev_twist_ptr_ = msg; } + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(Twist2Accel) diff --git a/localization/twist2accel/src/twist2accel_node.cpp b/localization/twist2accel/src/twist2accel_node.cpp deleted file mode 100644 index ce8ed31db5c32..0000000000000 --- a/localization/twist2accel/src/twist2accel_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2022 TIER IV -// -// 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 "twist2accel/twist2accel.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - auto node = std::make_shared("twist2accel", node_options); - - rclcpp::spin(node); - - return 0; -} diff --git a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt similarity index 88% rename from planning/behavior_path_external_request_lane_change_module/CMakeLists.txt rename to planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt index 86dc0ccb61330..662d36618e766 100644 --- a/planning/behavior_path_external_request_lane_change_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_external_request_lane_change_module/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_path_external_request_lane_change_module) +project(autoware_behavior_path_external_request_lane_change_module) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_path_external_request_lane_change_module/package.xml b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml similarity index 89% rename from planning/behavior_path_external_request_lane_change_module/package.xml rename to planning/autoware_behavior_path_external_request_lane_change_module/package.xml index 16216aa1f71e9..c08eb20a2b589 100644 --- a/planning/behavior_path_external_request_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_external_request_lane_change_module/package.xml @@ -1,9 +1,9 @@ - behavior_path_external_request_lane_change_module + autoware_behavior_path_external_request_lane_change_module 0.1.0 - The behavior_path_external_request_lane_change_module package + The autoware_behavior_path_external_request_lane_change_module package Fumiya Watanabe Zulfaqar Azmi diff --git a/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml new file mode 100644 index 0000000000000..d13dff53e0836 --- /dev/null +++ b/planning/autoware_behavior_path_external_request_lane_change_module/plugins.xml @@ -0,0 +1,4 @@ + + + + diff --git a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp similarity index 75% rename from planning/behavior_path_external_request_lane_change_module/src/manager.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp index 3cfe7aa51f0f1..130cbff29bcf3 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/manager.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.cpp @@ -12,13 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/manager.hpp" +#include "manager.hpp" -#include "behavior_path_external_request_lane_change_module/scene.hpp" #include "behavior_path_lane_change_module/interface.hpp" +#include "scene.hpp" -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeInterface; std::unique_ptr ExternalRequestLaneChangeRightModuleManager::createNewSceneModuleInstance() @@ -38,12 +39,12 @@ ExternalRequestLaneChangeLeftModuleManager::createNewSceneModuleInstance() std::make_unique(parameters_, direction_)); } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) PLUGINLIB_EXPORT_CLASS( - behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, - behavior_path_planner::SceneModuleManagerInterface) + autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + ::behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp similarity index 82% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp index dfcc4f61d8241..a4f3dce188c73 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/manager.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/manager.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#ifndef MANAGER_HPP_ +#define MANAGER_HPP_ #include "behavior_path_lane_change_module/manager.hpp" #include "route_handler/route_handler.hpp" @@ -23,8 +23,12 @@ #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleManager; +using ::behavior_path_planner::LaneChangeModuleType; +using ::behavior_path_planner::SceneModuleInterface; + class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager { public: @@ -49,6 +53,6 @@ class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManage } std::unique_ptr createNewSceneModuleInstance() override; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__MANAGER_HPP_ +#endif // MANAGER_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp similarity index 84% rename from planning/behavior_path_external_request_lane_change_module/src/scene.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp index 913266bf79fa3..2ba5a5aea34d4 100644 --- a/planning/behavior_path_external_request_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.cpp @@ -12,17 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_external_request_lane_change_module/scene.hpp" +#include "scene.hpp" #include #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::LaneChangeModuleType; + ExternalRequestLaneChange::ExternalRequestLaneChange( const std::shared_ptr & parameters, Direction direction) : NormalLaneChange(parameters, LaneChangeModuleType::EXTERNAL_REQUEST, direction) { } -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp similarity index 80% rename from planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp rename to planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp index c23d6f2f3996c..d2eb20b048a5d 100644 --- a/planning/behavior_path_external_request_lane_change_module/include/behavior_path_external_request_lane_change_module/scene.hpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/src/scene.hpp @@ -12,15 +12,19 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ -#define BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#ifndef SCENE_HPP_ +#define SCENE_HPP_ #include "behavior_path_lane_change_module/scene.hpp" #include -namespace behavior_path_planner +namespace autoware::behavior_path_planner { +using ::behavior_path_planner::Direction; +using ::behavior_path_planner::LaneChangeParameters; +using ::behavior_path_planner::NormalLaneChange; + class ExternalRequestLaneChange : public NormalLaneChange { public: @@ -33,6 +37,6 @@ class ExternalRequestLaneChange : public NormalLaneChange ExternalRequestLaneChange & operator=(ExternalRequestLaneChange &&) = delete; ~ExternalRequestLaneChange() override = default; }; -} // namespace behavior_path_planner +} // namespace autoware::behavior_path_planner -#endif // BEHAVIOR_PATH_EXTERNAL_REQUEST_LANE_CHANGE_MODULE__SCENE_HPP_ +#endif // SCENE_HPP_ diff --git a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp similarity index 94% rename from planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp rename to planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp index 1eb5118cd94b2..201d01e9b7fa0 100644 --- a/planning/behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/autoware_behavior_path_external_request_lane_change_module/test/test_behavior_path_planner_node_interface.cpp @@ -22,7 +22,7 @@ #include #include -using behavior_path_planner::BehaviorPathPlannerNode; +using ::behavior_path_planner::BehaviorPathPlannerNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() @@ -51,8 +51,10 @@ std::shared_ptr generateNode() ament_index_cpp::get_package_share_directory("behavior_path_lane_change_module"); std::vector module_names; - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); - module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back( + "autoware::behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/behavior_path_external_request_lane_change_module/plugins.xml b/planning/behavior_path_external_request_lane_change_module/plugins.xml deleted file mode 100644 index f3cc686837c38..0000000000000 --- a/planning/behavior_path_external_request_lane_change_module/plugins.xml +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/planning/behavior_path_lane_change_module/plugins.xml b/planning/behavior_path_lane_change_module/plugins.xml index a598bc8176938..7df36919d8d54 100644 --- a/planning/behavior_path_lane_change_module/plugins.xml +++ b/planning/behavior_path_lane_change_module/plugins.xml @@ -1,6 +1,6 @@ - - + + diff --git a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp index 1aab58ac2ff6e..df48b48d51241 100644 --- a/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp +++ b/planning/behavior_path_planner_common/src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp @@ -110,6 +110,14 @@ void OccupancyGridBasedCollisionDetector::setMap(const nav_msgs::msg::OccupancyG } } +static IndexXY position2index( + const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Point & position_local) +{ + const int index_x = position_local.x / costmap.info.resolution; + const int index_y = position_local.y / costmap.info.resolution; + return {index_x, index_y}; +} + void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( int theta_index, std::vector & indexes_2d) { @@ -131,12 +139,11 @@ void OccupancyGridBasedCollisionDetector::computeCollisionIndexes( const double offset_x = std::cos(base_theta) * x - std::sin(base_theta) * y; const double offset_y = std::sin(base_theta) * x + std::cos(base_theta) * y; - geometry_msgs::msg::Pose pose_local; - pose_local.position.x = base_pose.position.x + offset_x; - pose_local.position.y = base_pose.position.y + offset_y; + geometry_msgs::msg::Point position_local; + position_local.x = base_pose.position.x + offset_x; + position_local.y = base_pose.position.y + offset_y; - const auto index = pose2index(costmap_, pose_local, param_.theta_size); - const auto index_2d = IndexXY{index.x, index.y}; + const auto index_2d = position2index(costmap_, position_local); indexes_2d.push_back(index_2d); };