diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d2d6fa2c56343..2eefee5606ab2 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -153,6 +153,7 @@ planning/autoware_behavior_velocity_run_out_module/** kosuke.takeuchi@tier4.jp m planning/autoware_behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/autoware_behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/autoware_behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp +planning/autoware_costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@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/autoware_path_optimizer/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/autoware_planning_test_manager/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp @@ -178,11 +179,11 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp 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/autoware_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/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/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@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 diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index fb98d321fde88..6745976aa543a 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,40 +74,3 @@ jobs: - name: Show disk space after the tasks run: df -h - - clang-tidy-differential: - runs-on: ubuntu-latest - container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda - steps: - - name: Check out repository - uses: actions/checkout@v4 - with: - fetch-depth: 0 - - - name: Show disk space before the tasks - run: df -h - - - name: Remove exec_depend - uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 - - - name: Get modified packages - id: get-modified-packages - uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 - - - name: Get modified files - id: get-modified-files - uses: tj-actions/changed-files@v35 - with: - files: | - **/*.cpp - **/*.hpp - - - name: Run clang-tidy - if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} - uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 - with: - rosdistro: humble - target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} - target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} - clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy - build-depends-repos: build_depends.repos diff --git a/.github/workflows/clang-tidy-differential.yaml b/.github/workflows/clang-tidy-differential.yaml new file mode 100644 index 0000000000000..f7ec81e3394e5 --- /dev/null +++ b/.github/workflows/clang-tidy-differential.yaml @@ -0,0 +1,51 @@ +name: clang-tidy-differential + +on: + pull_request: + types: + - opened + - synchronize + - labeled + +jobs: + prevent-no-label-execution: + uses: autowarefoundation/autoware-github-actions/.github/workflows/prevent-no-label-execution.yaml@v1 + with: + label: tag:run-clang-tidy-differential + + clang-tidy-differential: + runs-on: ubuntu-latest + container: ghcr.io/autowarefoundation/autoware:latest-prebuilt-cuda + steps: + - name: Check out repository + uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Show disk space before the tasks + run: df -h + + - name: Remove exec_depend + uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 + + - name: Get modified packages + id: get-modified-packages + uses: autowarefoundation/autoware-github-actions/get-modified-packages@v1 + + - name: Get modified files + id: get-modified-files + uses: tj-actions/changed-files@v35 + with: + files: | + **/*.cpp + **/*.hpp + + - name: Run clang-tidy + if: ${{ steps.get-modified-files.outputs.all_changed_files != '' }} + uses: autowarefoundation/autoware-github-actions/clang-tidy@v1 + with: + rosdistro: humble + target-packages: ${{ steps.get-modified-packages.outputs.modified-packages }} + target-files: ${{ steps.get-modified-files.outputs.all_changed_files }} + clang-tidy-config-url: https://raw.githubusercontent.com/autowarefoundation/autoware/main/.clang-tidy + build-depends-repos: build_depends.repos diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index 97bf6414189ab..1d01732454daf 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -32,7 +32,7 @@ - + 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 4cf0c541510ed..779ee5c7af34a 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 @@ -101,7 +101,7 @@ diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index b5173d6a98b2e..4ed38df2d3f09 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -62,7 +62,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 98315919b540a..db5a9a0093e7c 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -4,7 +4,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 6a7e00154256a..f96cac4f017a6 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 @@ -27,7 +27,7 @@ - + diff --git a/launch/tier4_planning_launch/package.xml b/launch/tier4_planning_launch/package.xml index 7b03097e55e10..e5def4c7aed0c 100644 --- a/launch/tier4_planning_launch/package.xml +++ b/launch/tier4_planning_launch/package.xml @@ -58,14 +58,15 @@ autoware_cmake autoware_behavior_velocity_planner + autoware_costmap_generator autoware_external_velocity_limit_selector autoware_path_optimizer autoware_planning_topic_converter + autoware_planning_validator autoware_remaining_distance_time_calculator autoware_surround_obstacle_checker autoware_velocity_smoother behavior_path_planner - costmap_generator external_cmd_selector freespace_planner glog_component @@ -73,7 +74,6 @@ obstacle_cruise_planner obstacle_stop_planner planning_evaluator - planning_validator scenario_selector ament_lint_auto diff --git a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp index 49b881900b997..c5b7b7a592013 100644 --- a/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/diagnostics_module.hpp @@ -30,14 +30,14 @@ class DiagnosticsModule public: DiagnosticsModule(rclcpp::Node * node, const std::string & diagnostic_name); void clear(); - void addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg); + void add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg); template - void addKeyValue(const std::string & key, const T & value); - void updateLevelAndMessage(const int8_t level, const std::string & message); + void add_key_value(const std::string & key, const T & value); + void update_level_and_message(const int8_t level, const std::string & message); void publish(const rclcpp::Time & publish_time_stamp); private: - diagnostic_msgs::msg::DiagnosticArray createDiagnosticsArray( + [[nodiscard]] diagnostic_msgs::msg::DiagnosticArray create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const; rclcpp::Clock::SharedPtr clock_; @@ -47,18 +47,18 @@ class DiagnosticsModule }; template -void DiagnosticsModule::addKeyValue(const std::string & key, const T & value) +void DiagnosticsModule::add_key_value(const std::string & key, const T & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = std::to_string(value); - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value); +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value); template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value); +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value); } // namespace autoware::gyro_odometer 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 3b2da504f938a..f85640ab2c523 100644 --- a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp +++ b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp @@ -47,14 +47,13 @@ class GyroOdometerNode : public rclcpp::Node public: explicit GyroOdometerNode(const rclcpp::NodeOptions & node_options); - ~GyroOdometerNode(); private: - void callbackVehicleTwist( + void callback_vehicle_twist( const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr); - void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); - void concatGyroAndOdometer(); - void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); + void callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); + void concat_gyro_and_odometer(); + void publish_data(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw); rclcpp::Subscription::SharedPtr vehicle_twist_sub_; diff --git a/localization/gyro_odometer/src/diagnostics_module.cpp b/localization/gyro_odometer/src/diagnostics_module.cpp index 9d88d8e6833ed..5d687943cef53 100644 --- a/localization/gyro_odometer/src/diagnostics_module.cpp +++ b/localization/gyro_odometer/src/diagnostics_module.cpp @@ -44,7 +44,7 @@ void DiagnosticsModule::clear() diagnostics_status_msg_.message = ""; } -void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_value_msg) +void DiagnosticsModule::add_key_value(const diagnostic_msgs::msg::KeyValue & key_value_msg) { auto it = std::find_if( std::begin(diagnostics_status_msg_.values), std::end(diagnostics_status_msg_.values), @@ -58,24 +58,24 @@ void DiagnosticsModule::addKeyValue(const diagnostic_msgs::msg::KeyValue & key_v } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const std::string & value) +void DiagnosticsModule::add_key_value(const std::string & key, const std::string & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value; - addKeyValue(key_value); + add_key_value(key_value); } template <> -void DiagnosticsModule::addKeyValue(const std::string & key, const bool & value) +void DiagnosticsModule::add_key_value(const std::string & key, const bool & value) { diagnostic_msgs::msg::KeyValue key_value; key_value.key = key; key_value.value = value ? "True" : "False"; - addKeyValue(key_value); + add_key_value(key_value); } -void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::string & message) +void DiagnosticsModule::update_level_and_message(const int8_t level, const std::string & message) { if ((level > diagnostic_msgs::msg::DiagnosticStatus::OK)) { if (!diagnostics_status_msg_.message.empty()) { @@ -90,10 +90,10 @@ void DiagnosticsModule::updateLevelAndMessage(const int8_t level, const std::str void DiagnosticsModule::publish(const rclcpp::Time & publish_time_stamp) { - diagnostics_pub_->publish(createDiagnosticsArray(publish_time_stamp)); + diagnostics_pub_->publish(create_diagnostics_array(publish_time_stamp)); } -diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::createDiagnosticsArray( +diagnostic_msgs::msg::DiagnosticArray DiagnosticsModule::create_diagnostics_array( const rclcpp::Time & publish_time_stamp) const { diagnostic_msgs::msg::DiagnosticArray diagnostics_msg; diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp index 1852d70bced71..0ec479770740f 100644 --- a/localization/gyro_odometer/src/gyro_odometer_core.cpp +++ b/localization/gyro_odometer/src/gyro_odometer_core.cpp @@ -31,13 +31,13 @@ namespace autoware::gyro_odometer { -std::array transformCovariance(const std::array & cov) +std::array transform_covariance(const std::array & cov) { using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; double max_cov = std::max({cov[COV_IDX::X_X], cov[COV_IDX::Y_Y], cov[COV_IDX::Z_Z]}); - std::array cov_transformed; + std::array cov_transformed = {}; cov_transformed.fill(0.); cov_transformed[COV_IDX::X_X] = max_cov; cov_transformed[COV_IDX::Y_Y] = max_cov; @@ -57,11 +57,11 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) vehicle_twist_sub_ = create_subscription( "vehicle/twist_with_covariance", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackVehicleTwist, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_vehicle_twist, this, std::placeholders::_1)); imu_sub_ = create_subscription( "imu", rclcpp::QoS{100}, - std::bind(&GyroOdometerNode::callbackImu, this, std::placeholders::_1)); + std::bind(&GyroOdometerNode::callback_imu, this, std::placeholders::_1)); twist_raw_pub_ = create_publisher("twist_raw", rclcpp::QoS{10}); twist_with_covariance_raw_pub_ = create_publisher( @@ -76,49 +76,46 @@ GyroOdometerNode::GyroOdometerNode(const rclcpp::NodeOptions & node_options) // TODO(YamatoAndo) createTimer } -GyroOdometerNode::~GyroOdometerNode() -{ -} - -void GyroOdometerNode::callbackVehicleTwist( - const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_ptr) +void GyroOdometerNode::callback_vehicle_twist( + const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( - "topic_time_stamp", static_cast(vehicle_twist_ptr->header.stamp).nanoseconds()); + diagnostics_->add_key_value( + "topic_time_stamp", + static_cast(vehicle_twist_msg_ptr->header.stamp).nanoseconds()); vehicle_twist_arrived_ = true; - latest_vehicle_twist_ros_time_ = vehicle_twist_ptr->header.stamp; - vehicle_twist_queue_.push_back(*vehicle_twist_ptr); - concatGyroAndOdometer(); + latest_vehicle_twist_ros_time_ = vehicle_twist_msg_ptr->header.stamp; + vehicle_twist_queue_.push_back(*vehicle_twist_msg_ptr); + concat_gyro_and_odometer(); - diagnostics_->publish(vehicle_twist_ptr->header.stamp); + diagnostics_->publish(vehicle_twist_msg_ptr->header.stamp); } -void GyroOdometerNode::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) +void GyroOdometerNode::callback_imu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr) { diagnostics_->clear(); - diagnostics_->addKeyValue( + diagnostics_->add_key_value( "topic_time_stamp", static_cast(imu_msg_ptr->header.stamp).nanoseconds()); imu_arrived_ = true; latest_imu_ros_time_ = imu_msg_ptr->header.stamp; gyro_queue_.push_back(*imu_msg_ptr); - concatGyroAndOdometer(); + concat_gyro_and_odometer(); diagnostics_->publish(imu_msg_ptr->header.stamp); } -void GyroOdometerNode::concatGyroAndOdometer() +void GyroOdometerNode::concat_gyro_and_odometer() { // check arrive first topic - diagnostics_->addKeyValue("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); - diagnostics_->addKeyValue("is_arrived_first_imu", imu_arrived_); + diagnostics_->add_key_value("is_arrived_first_vehicle_twist", vehicle_twist_arrived_); + diagnostics_->add_key_value("is_arrived_first_imu", imu_arrived_); if (!vehicle_twist_arrived_) { std::stringstream message; message << "Twist msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -129,7 +126,7 @@ void GyroOdometerNode::concatGyroAndOdometer() std::stringstream message; message << "Imu msg is not subscribed"; RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::WARN, message.str()); vehicle_twist_queue_.clear(); @@ -141,14 +138,14 @@ void GyroOdometerNode::concatGyroAndOdometer() const double vehicle_twist_dt = std::abs((this->now() - latest_vehicle_twist_ros_time_).seconds()); const double imu_dt = std::abs((this->now() - latest_imu_ros_time_).seconds()); - diagnostics_->addKeyValue("vehicle_twist_time_stamp_dt", vehicle_twist_dt); - diagnostics_->addKeyValue("imu_time_stamp_dt", imu_dt); + diagnostics_->add_key_value("vehicle_twist_time_stamp_dt", vehicle_twist_dt); + diagnostics_->add_key_value("imu_time_stamp_dt", imu_dt); if (vehicle_twist_dt > message_timeout_sec_) { const std::string message = fmt::format( "Vehicle twist msg is timeout. vehicle_twist_dt: {}[sec], tolerance {}[sec]", vehicle_twist_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -158,7 +155,7 @@ void GyroOdometerNode::concatGyroAndOdometer() const std::string message = fmt::format( "Imu msg is timeout. imu_dt: {}[sec], tolerance {}[sec]", imu_dt, message_timeout_sec_); RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message); - diagnostics_->updateLevelAndMessage(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); + diagnostics_->update_level_and_message(diagnostic_msgs::msg::DiagnosticStatus::ERROR, message); vehicle_twist_queue_.clear(); gyro_queue_.clear(); @@ -166,8 +163,8 @@ void GyroOdometerNode::concatGyroAndOdometer() } // check queue size - diagnostics_->addKeyValue("vehicle_twist_queue_size", vehicle_twist_queue_.size()); - diagnostics_->addKeyValue("imu_queue_size", gyro_queue_.size()); + diagnostics_->add_key_value("vehicle_twist_queue_size", vehicle_twist_queue_.size()); + diagnostics_->add_key_value("imu_queue_size", gyro_queue_.size()); if (vehicle_twist_queue_.empty()) { // not output error and clear queue return; @@ -182,13 +179,13 @@ void GyroOdometerNode::concatGyroAndOdometer() transform_listener_->getLatestTransform(gyro_queue_.front().header.frame_id, output_frame_); const bool is_succeed_transform_imu = (tf_imu2base_ptr != nullptr); - diagnostics_->addKeyValue("is_succeed_transform_imu", is_succeed_transform_imu); + diagnostics_->add_key_value("is_succeed_transform_imu", is_succeed_transform_imu); if (!is_succeed_transform_imu) { std::stringstream message; message << "Please publish TF " << output_frame_ << " to " << gyro_queue_.front().header.frame_id; RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, message.str()); - diagnostics_->updateLevelAndMessage( + diagnostics_->update_level_and_message( diagnostic_msgs::msg::DiagnosticStatus::ERROR, message.str()); vehicle_twist_queue_.clear(); @@ -208,7 +205,7 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro.header.frame_id = output_frame_; gyro.angular_velocity = transformed_angular_velocity.vector; - gyro.angular_velocity_covariance = transformCovariance(gyro.angular_velocity_covariance); + gyro.angular_velocity_covariance = transform_covariance(gyro.angular_velocity_covariance); } using COV_IDX_XYZ = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; @@ -223,8 +220,8 @@ void GyroOdometerNode::concatGyroAndOdometer() vx_mean += vehicle_twist.twist.twist.linear.x; vx_covariance_original += vehicle_twist.twist.covariance[0 * 6 + 0]; } - vx_mean /= vehicle_twist_queue_.size(); - vx_covariance_original /= vehicle_twist_queue_.size(); + vx_mean /= static_cast(vehicle_twist_queue_.size()); + vx_covariance_original /= static_cast(vehicle_twist_queue_.size()); for (const auto & gyro : gyro_queue_) { gyro_mean.x += gyro.angular_velocity.x; @@ -234,12 +231,12 @@ void GyroOdometerNode::concatGyroAndOdometer() gyro_covariance_original.y += gyro.angular_velocity_covariance[COV_IDX_XYZ::Y_Y]; gyro_covariance_original.z += gyro.angular_velocity_covariance[COV_IDX_XYZ::Z_Z]; } - gyro_mean.x /= gyro_queue_.size(); - gyro_mean.y /= gyro_queue_.size(); - gyro_mean.z /= gyro_queue_.size(); - gyro_covariance_original.x /= gyro_queue_.size(); - gyro_covariance_original.y /= gyro_queue_.size(); - gyro_covariance_original.z /= gyro_queue_.size(); + gyro_mean.x /= static_cast(gyro_queue_.size()); + gyro_mean.y /= static_cast(gyro_queue_.size()); + gyro_mean.z /= static_cast(gyro_queue_.size()); + gyro_covariance_original.x /= static_cast(gyro_queue_.size()); + gyro_covariance_original.y /= static_cast(gyro_queue_.size()); + gyro_covariance_original.z /= static_cast(gyro_queue_.size()); // concat geometry_msgs::msg::TwistWithCovarianceStamped twist_with_cov; @@ -257,23 +254,23 @@ void GyroOdometerNode::concatGyroAndOdometer() // From a statistical point of view, here we reduce the covariances according to the number of // observed data twist_with_cov.twist.covariance[COV_IDX_XYZRPY::X_X] = - vx_covariance_original / vehicle_twist_queue_.size(); + vx_covariance_original / static_cast(vehicle_twist_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Y_Y] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::Z_Z] = 100000.0; twist_with_cov.twist.covariance[COV_IDX_XYZRPY::ROLL_ROLL] = - gyro_covariance_original.x / gyro_queue_.size(); + gyro_covariance_original.x / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::PITCH_PITCH] = - gyro_covariance_original.y / gyro_queue_.size(); + gyro_covariance_original.y / static_cast(gyro_queue_.size()); twist_with_cov.twist.covariance[COV_IDX_XYZRPY::YAW_YAW] = - gyro_covariance_original.z / gyro_queue_.size(); + gyro_covariance_original.z / static_cast(gyro_queue_.size()); - publishData(twist_with_cov); + publish_data(twist_with_cov); vehicle_twist_queue_.clear(); gyro_queue_.clear(); } -void GyroOdometerNode::publishData( +void GyroOdometerNode::publish_data( const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw) { geometry_msgs::msg::TwistStamped twist_raw; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp index 54e1d320319d3..f55278998f5f0 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.cpp @@ -17,7 +17,7 @@ using geometry_msgs::msg::TwistWithCovarianceStamped; using sensor_msgs::msg::Imu; -Imu generateSampleImu() +Imu generate_sample_imu() { Imu imu; imu.header.frame_id = "base_link"; @@ -27,7 +27,7 @@ Imu generateSampleImu() return imu; } -TwistWithCovarianceStamped generateSampleVelocity() +TwistWithCovarianceStamped generate_sample_velocity() { TwistWithCovarianceStamped twist; twist.header.frame_id = "base_link"; @@ -35,7 +35,7 @@ TwistWithCovarianceStamped generateSampleVelocity() return twist; } -rclcpp::NodeOptions getNodeOptionsWithDefaultParams() +rclcpp::NodeOptions get_node_options_with_default_params() { rclcpp::NodeOptions node_options; diff --git a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp index 6e3aff0b841a9..9da344e410e61 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_helper.hpp @@ -20,11 +20,8 @@ #include #include -using geometry_msgs::msg::TwistWithCovarianceStamped; -using sensor_msgs::msg::Imu; - -Imu generateSampleImu(); -TwistWithCovarianceStamped generateSampleVelocity(); -rclcpp::NodeOptions getNodeOptionsWithDefaultParams(); +sensor_msgs::msg::Imu generate_sample_imu(); +geometry_msgs::msg::TwistWithCovarianceStamped generate_sample_velocity(); +rclcpp::NodeOptions get_node_options_with_default_params(); #endif // TEST_GYRO_ODOMETER_HELPER_HPP_ diff --git a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp index b7849ef03bfc5..fc331a638a1dd 100644 --- a/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp +++ b/localization/gyro_odometer/test/test_gyro_odometer_pubsub.cpp @@ -50,19 +50,22 @@ class VelocityGenerator : public rclcpp::Node class GyroOdometerValidator : public rclcpp::Node { public: - GyroOdometerValidator() : Node("gyro_odometer_validator"), received_latest_twist_ptr(nullptr) - { - twist_sub = create_subscription( - "/twist_with_covariance", 1, [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { + GyroOdometerValidator() + : Node("gyro_odometer_validator"), + twist_sub(create_subscription( + "/twist_with_covariance", 1, + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { received_latest_twist_ptr = msg; - }); + })), + received_latest_twist_ptr(nullptr) + { } rclcpp::Subscription::SharedPtr twist_sub; TwistWithCovarianceStamped::ConstSharedPtr received_latest_twist_ptr; }; -void spinSome(rclcpp::Node::SharedPtr node_ptr) +void wait_spin_some(rclcpp::Node::SharedPtr node_ptr) { for (int i = 0; i < 50; ++i) { rclcpp::spin_some(node_ptr); @@ -70,7 +73,7 @@ void spinSome(rclcpp::Node::SharedPtr node_ptr) } } -bool isTwistValid( +bool is_twist_valid( const TwistWithCovarianceStamped & twist, const TwistWithCovarianceStamped & twist_ground_truth) { if (twist.twist.twist.linear.x != twist_ground_truth.twist.twist.linear.x) { @@ -99,8 +102,8 @@ bool isTwistValid( // velocity data are provided TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) { - Imu input_imu = generateSampleImu(); - TwistWithCovarianceStamped input_velocity = generateSampleVelocity(); + Imu input_imu = generate_sample_imu(); + TwistWithCovarianceStamped input_velocity = generate_sample_velocity(); TwistWithCovarianceStamped expected_output_twist; expected_output_twist.twist.twist.linear.x = input_velocity.twist.twist.linear.x; @@ -108,8 +111,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( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto velocity_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); @@ -120,13 +123,13 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) velocity_generator->vehicle_velocity_pub->publish(input_velocity); // gyro_odometer receives IMU and velocity, and publishes the fused twist data. - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node receives the fused twist data and store in "received_latest_twist_ptr". - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_FALSE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); - EXPECT_TRUE(isTwistValid( + EXPECT_TRUE(is_twist_valid( *(gyro_odometer_validator_node->received_latest_twist_ptr), expected_output_twist)); } @@ -134,20 +137,20 @@ TEST(GyroOdometer, TestGyroOdometerWithImuAndVelocity) // Verify that the gyro_odometer does NOT publish any outputs when only IMU is provided TEST(GyroOdometer, TestGyroOdometerImuOnly) { - Imu input_imu = generateSampleImu(); + Imu input_imu = generate_sample_imu(); - auto gyro_odometer_node = - std::make_shared(getNodeOptionsWithDefaultParams()); + auto gyro_odometer_node = std::make_shared( + get_node_options_with_default_params()); auto imu_generator = std::make_shared(); auto gyro_odometer_validator_node = std::make_shared(); imu_generator->imu_pub->publish(input_imu); // gyro_odometer receives IMU - spinSome(gyro_odometer_node); + wait_spin_some(gyro_odometer_node); // validator node waits for the output fused twist from gyro_odometer - spinSome(gyro_odometer_validator_node); + wait_spin_some(gyro_odometer_validator_node); EXPECT_TRUE(gyro_odometer_validator_node->received_latest_twist_ptr == nullptr); } diff --git a/localization/pose_instability_detector/README.md b/localization/pose_instability_detector/README.md index 89cf6ca3be684..4ced0fa8eb97b 100644 --- a/localization/pose_instability_detector/README.md +++ b/localization/pose_instability_detector/README.md @@ -1,20 +1,111 @@ # pose_instability_detector -The `pose_instability_detector` package includes a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). +The `pose_instability_detector` is a node designed to monitor the stability of `/localization/kinematic_state`, which is an output topic of the Extended Kalman Filter (EKF). This node triggers periodic timer callbacks to compare two poses: -- The pose obtained by integrating the twist values from the last received message on `/localization/kinematic_state` over a duration specified by `interval_sec`. +- The pose calculated by dead reckoning starting from the pose of `/localization/kinematic_state` obtained `timer_period` seconds ago. - The latest pose from `/localization/kinematic_state`. The results of this comparison are then output to the `/diagnostics` topic. +![overview](./media/pose_instability_detector_overview.png) + +![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) + If this node outputs WARN messages to `/diagnostics`, it means that the EKF output is significantly different from the integrated twist values. +In other words, WARN outputs indicate that the vehicle has moved to a place outside the expected range based on the twist values. This discrepancy suggests that there may be an issue with either the estimated pose or the input twist. -The following diagram provides an overview of what the timeline of this process looks like: +The following diagram provides an overview of how the procedure looks like: + +![procedure](./media/pose_instabilty_detector_procedure.svg) + +## Dead reckoning algorithm + +Dead reckoning is a method of estimating the position of a vehicle based on its previous position and velocity. +The procedure for dead reckoning is as follows: + +1. Capture the necessary twist values from the `/input/twist` topic. +2. Integrate the twist values to calculate the pose transition. +3. Apply the pose transition to the previous pose to obtain the current pose. + +### Collecting twist values + +The `pose_instability_detector` node collects the twist values from the `~/input/twist` topic to perform dead reckoning. +Ideally, `pose_instability_detector` needs the twist values between the previous pose and the current pose. +Therefore, `pose_instability_detector` snips the twist buffer and apply interpolations and extrapolations to obtain the twist values at the desired time. + +![how_to_snip_necessary_twist](./media/how_to_snip_twist.png) + +### Linear transition and angular transition + +After the twist values are collected, the node calculates the linear transition and angular transition based on the twist values and add them to the previous pose. + +## Threshold definition + +The `pose_instability_detector` node compares the pose calculated by dead reckoning with the latest pose from the EKF output. +These two pose are ideally the same, but in reality, they are not due to the error in the twist values the pose observation. +If these two poses are significantly different so that the absolute value exceeds the threshold, the node outputs a WARN message to the `/diagnostics` topic. +There are six thresholds (x, y, z, roll, pitch, and yaw) to determine whether the poses are significantly different, and these thresholds are determined by the following subsections. + +### `diff_position_x` + +This threshold examines the difference in the longitudinal axis between the two poses, and check whether the vehicle goes beyond the expected error. +This threshold is a sum of "maximum longitudinal error due to velocity scale factor error" and "pose estimation error tolerance". + +$$ +\tau_x = v_{\rm max}\frac{\beta_v}{100} \Delta t + \epsilon_x\\ +$$ -![timeline](./media/timeline.drawio.svg) +| Symbol | Description | Unit | +| ------------- | -------------------------------------------------------------------------------- | ----- | +| $\tau_x$ | Threshold for the difference in the longitudinal axis | $m$ | +| $v_{\rm max}$ | Maximum velocity | $m/s$ | +| $\beta_v$ | Scale factor tolerance for the maximum velocity | $\%$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_x$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the longitudinal axis | $m$ | + +### `diff_position_y` and `diff_position_z` + +These thresholds examine the difference in the lateral and vertical axes between the two poses, and check whether the vehicle goes beyond the expected error. +The `pose_instability_detector` calculates the possible range where the vehicle goes, and get the maximum difference between the nominal dead reckoning pose and the maximum limit pose. + +![lateral_threshold_calculation](./media/lateral_threshold_calculation.png) + +Addition to this, the `pose_instability_detector` node considers the pose estimation error tolerance to determine the threshold. + +$$ +\tau_y = l + \epsilon_y +$$ + +| Symbol | Description | Unit | +| ------------ | ----------------------------------------------------------------------------------------------- | ---- | +| $\tau_y$ | Threshold for the difference in the lateral axis | $m$ | +| $l$ | Maximum lateral distance described in the image above (See the appendix how this is calculated) | $m$ | +| $\epsilon_y$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the lateral axis | $m$ | + +Note that `pose_instability_detector` sets the threshold for the vertical axis as the same as the lateral axis. Only the pose estimator error tolerance is different. + +### `diff_angle_x`, `diff_angle_y`, and `diff_angle_z` + +These thresholds examine the difference in the roll, pitch, and yaw angles between the two poses. +This threshold is a sum of "maximum angular error due to velocity scale factor error and bias error" and "pose estimation error tolerance". + +$$ +\tau_\phi = \tau_\theta = \tau_\psi = \left(\omega_{\rm max}\frac{\beta_\omega}{100} + b \right) \Delta t + \epsilon_\psi +$$ + +| Symbol | Description | Unit | +| ------------------ | ------------------------------------------------------------------------ | ------------- | +| $\tau_\phi$ | Threshold for the difference in the roll angle | ${\rm rad}$ | +| $\tau_\theta$ | Threshold for the difference in the pitch angle | ${\rm rad}$ | +| $\tau_\psi$ | Threshold for the difference in the yaw angle | ${\rm rad}$ | +| $\omega_{\rm max}$ | Maximum angular velocity | ${\rm rad}/s$ | +| $\beta_\omega$ | Scale factor tolerance for the maximum angular velocity | $\%$ | +| $b$ | Bias tolerance of the angular velocity | ${\rm rad}/s$ | +| $\Delta t$ | Time interval | $s$ | +| $\epsilon_\psi$ | Pose estimator (e. g. ndt_scan_matcher) error tolerance in the yaw angle | ${\rm rad}$ | ## Parameters @@ -34,4 +125,44 @@ The following diagram provides an overview of what the timeline of this process | `~/debug/diff_pose` | geometry_msgs::msg::PoseStamped | diff_pose | | `/diagnostics` | diagnostic_msgs::msg::DiagnosticArray | Diagnostics | -![rqt_runtime_monitor](./media/rqt_runtime_monitor.png) +## Appendix + +On calculating the maximum lateral distance $l$, the `pose_instability_detector` node will estimate the following poses. + +| Pose | heading velocity $v$ | angular velocity $\omega$ | +| ------------------------------- | ------------------------------------------------ | -------------------------------------------------------------- | +| Nominal dead reckoning pose | $v_{\rm max}$ | $\omega_{\rm max}$ | +| Dead reckoning pose of corner A | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner B | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1+\frac{\beta_\omega}{100}\right) \omega_{\rm max} + b$ | +| Dead reckoning pose of corner C | $\left(1-\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | +| Dead reckoning pose of corner D | $\left(1+\frac{\beta_v}{100}\right) v_{\rm max}$ | $\left(1-\frac{\beta_\omega}{100}\right) \omega_{\rm max} - b$ | + +Given a heading velocity $v$ and $\omega$, the 2D theoretical variation seen from the previous pose is calculated as follows: + +$$ +\begin{align*} +\left[ + \begin{matrix} + \Delta x\\ + \Delta y + \end{matrix} +\right] +&= +\left[ + \begin{matrix} + \int_{0}^{\Delta t} v \cos(\omega t) dt\\ + \int_{0}^{\Delta t} v \sin(\omega t) dt + \end{matrix} +\right] +\\ +&= +\left[ + \begin{matrix} + \frac{v}{\omega} \sin(\omega \Delta t)\\ + \frac{v}{\omega} \left(1 - \cos(\omega \Delta t)\right) + \end{matrix} +\right] +\end{align*} +$$ + +We calculate this variation for each corner and get the maximum value of the lateral distance $l$ by comparing the distance between the nominal dead reckoning pose and the corner poses. diff --git a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml index d94de020a4a12..d9b11b78885c9 100644 --- a/localization/pose_instability_detector/config/pose_instability_detector.param.yaml +++ b/localization/pose_instability_detector/config/pose_instability_detector.param.yaml @@ -1,9 +1,15 @@ /**: ros__parameters: - interval_sec: 0.5 # [sec] - threshold_diff_position_x: 1.0 # [m] - threshold_diff_position_y: 1.0 # [m] - threshold_diff_position_z: 1.0 # [m] - threshold_diff_angle_x: 1.0 # [rad] - threshold_diff_angle_y: 1.0 # [rad] - threshold_diff_angle_z: 1.0 # [rad] + timer_period: 0.5 # [sec] + + heading_velocity_maximum: 16.667 # [m/s] + heading_velocity_scale_factor_tolerance: 3.0 # [%] + + angular_velocity_maximum: 0.523 # [rad/s] + angular_velocity_scale_factor_tolerance: 0.2 # [%] + angular_velocity_bias_tolerance: 0.00698 # [rad/s] + + pose_estimator_longitudinal_tolerance: 0.11 # [m] + pose_estimator_lateral_tolerance: 0.11 # [m] + pose_estimator_vertical_tolerance: 0.11 # [m] + pose_estimator_angular_tolerance: 0.0175 # [rad] diff --git a/localization/pose_instability_detector/src/pose_instability_detector.hpp b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp similarity index 57% rename from localization/pose_instability_detector/src/pose_instability_detector.hpp rename to localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp index 761a10b7a6bf7..0a55a5005dde1 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.hpp +++ b/localization/pose_instability_detector/include/autoware/pose_instability_detector/pose_instability_detector.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POSE_INSTABILITY_DETECTOR_HPP_ -#define POSE_INSTABILITY_DETECTOR_HPP_ +#ifndef AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ +#define AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ #include @@ -22,6 +22,8 @@ #include #include +#include +#include #include class PoseInstabilityDetector : public rclcpp::Node @@ -37,13 +39,31 @@ class PoseInstabilityDetector : public rclcpp::Node using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; public: + struct ThresholdValues + { + double position_x; + double position_y; + double position_z; + double angle_x; + double angle_y; + double angle_z; + }; + explicit PoseInstabilityDetector(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + ThresholdValues calculate_threshold(double interval_sec); + void dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose); private: void callback_odometry(Odometry::ConstSharedPtr odometry_msg_ptr); void callback_twist(TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); void callback_timer(); + std::deque clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time); + // subscribers and timer rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr twist_sub_; @@ -54,17 +74,26 @@ class PoseInstabilityDetector : public rclcpp::Node rclcpp::Publisher::SharedPtr diagnostics_pub_; // parameters - const double threshold_diff_position_x_; - const double threshold_diff_position_y_; - const double threshold_diff_position_z_; - const double threshold_diff_angle_x_; - const double threshold_diff_angle_y_; - const double threshold_diff_angle_z_; + const double timer_period_; // [sec] + + ThresholdValues threshold_values_; + + const double heading_velocity_maximum_; // [m/s] + const double heading_velocity_scale_factor_tolerance_; // [%] + + const double angular_velocity_maximum_; // [rad/s] + const double angular_velocity_scale_factor_tolerance_; // [%] + const double angular_velocity_bias_tolerance_; // [rad/s] + + const double pose_estimator_longitudinal_tolerance_; // [m] + const double pose_estimator_lateral_tolerance_; // [m] + const double pose_estimator_vertical_tolerance_; // [m] + const double pose_estimator_angular_tolerance_; // [rad] // variables std::optional latest_odometry_ = std::nullopt; std::optional prev_odometry_ = std::nullopt; - std::vector twist_buffer_; + std::deque twist_buffer_; }; -#endif // POSE_INSTABILITY_DETECTOR_HPP_ +#endif // AUTOWARE__POSE_INSTABILITY_DETECTOR__POSE_INSTABILITY_DETECTOR_HPP_ diff --git a/localization/pose_instability_detector/media/how_to_snip_twist.png b/localization/pose_instability_detector/media/how_to_snip_twist.png new file mode 100644 index 0000000000000..6dc66a480e769 Binary files /dev/null and b/localization/pose_instability_detector/media/how_to_snip_twist.png differ diff --git a/localization/pose_instability_detector/media/lateral_threshold_calculation.png b/localization/pose_instability_detector/media/lateral_threshold_calculation.png new file mode 100644 index 0000000000000..cd919cdcb0383 Binary files /dev/null and b/localization/pose_instability_detector/media/lateral_threshold_calculation.png differ diff --git a/localization/pose_instability_detector/media/pose_instability_detector_overview.png b/localization/pose_instability_detector/media/pose_instability_detector_overview.png new file mode 100644 index 0000000000000..ea8f7a81700ae Binary files /dev/null and b/localization/pose_instability_detector/media/pose_instability_detector_overview.png differ diff --git a/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg new file mode 100644 index 0000000000000..ba45b1f52600b --- /dev/null +++ b/localization/pose_instability_detector/media/pose_instabilty_detector_procedure.svg @@ -0,0 +1,316 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ Get necessary subsequence +
+ from twist buffer +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Update +
+ + latest pose +
+ +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+ Calculate integration of +
+ the twist subsequence +
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Calculate relative difference between +
+
+
+ + latest pose +
+ +
+
and
+
+ previous pose + twist integration +
+
+
+
+
+ + + + + + + + + + + + + + + + +
+
+
+
+ Compare pose difference with thresholds +
+
+
+ Output results as + /diagnostics +
+
+
+
+
+ + + + + + + + + + + + +
+
+
+
+ + previous pose ← latest pose +
+ +
+
+
+
+ + + + + + + + + + + + +
+
+
+ Timer Callback +
+
+
+ + + + + + + + + + + + +
+
+
+ Odometry Subscription Callback +
+
+
+ + + + + + + diff --git a/localization/pose_instability_detector/media/rqt_runtime_monitor.png b/localization/pose_instability_detector/media/rqt_runtime_monitor.png index b3ad402e48ba7..9bad665db17e5 100644 Binary files a/localization/pose_instability_detector/media/rqt_runtime_monitor.png and b/localization/pose_instability_detector/media/rqt_runtime_monitor.png differ diff --git a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json index 560d39a2d5bff..53380f8e7f252 100644 --- a/localization/pose_instability_detector/schema/pose_instability_detector.schema.json +++ b/localization/pose_instability_detector/schema/pose_instability_detector.schema.json @@ -1,62 +1,83 @@ { "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for Pose Instability Detector Nodes", + "title": "Parameters for Pose Instability Detector Node", "type": "object", "definitions": { "pose_instability_detector_node": { "type": "object", "properties": { - "interval_sec": { + "timer_period": { "type": "number", "default": 0.5, "exclusiveMinimum": 0, - "description": "The interval of timer_callback in seconds." + "description": "The period of timer_callback (sec)." }, - "threshold_diff_position_x": { + "heading_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 16.667, "minimum": 0.0, - "description": "The threshold of diff_position x (m)." + "description": "The maximum of heading velocity (m/s)." }, - "threshold_diff_position_y": { + "heading_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 3.0, "minimum": 0.0, - "description": "The threshold of diff_position y (m)." + "description": "The tolerance of heading velocity scale factor (%)." }, - "threshold_diff_position_z": { + "angular_velocity_maximum": { "type": "number", - "default": 1.0, + "default": 0.523, "minimum": 0.0, - "description": "The threshold of diff_position z (m)." + "description": "The maximum of angular velocity (rad/s)." }, - "threshold_diff_angle_x": { + "angular_velocity_scale_factor_tolerance": { "type": "number", - "default": 1.0, + "default": 0.2, "minimum": 0.0, - "description": "The threshold of diff_angle x (rad)." + "description": "The tolerance of angular velocity scale factor (%)." }, - "threshold_diff_angle_y": { + "angular_velocity_bias_tolerance": { "type": "number", - "default": 1.0, + "default": 0.00698, "minimum": 0.0, - "description": "The threshold of diff_angle y (rad)." + "description": "The tolerance of angular velocity bias (rad/s)." }, - "threshold_diff_angle_z": { + "pose_estimator_longitudinal_tolerance": { "type": "number", - "default": 1.0, + "default": 0.11, "minimum": 0.0, - "description": "The threshold of diff_angle z (rad)." + "description": "The tolerance of longitudinal position of pose estimator (m)." + }, + "pose_estimator_lateral_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of lateral position of pose estimator (m)." + }, + "pose_estimator_vertical_tolerance": { + "type": "number", + "default": 0.11, + "minimum": 0.0, + "description": "The tolerance of vertical position of pose estimator (m)." + }, + "pose_estimator_angular_tolerance": { + "type": "number", + "default": 0.0175, + "minimum": 0.0, + "description": "The tolerance of roll angle of pose estimator (rad)." } }, "required": [ - "interval_sec", - "threshold_diff_position_x", - "threshold_diff_position_y", - "threshold_diff_position_z", - "threshold_diff_angle_x", - "threshold_diff_angle_y", - "threshold_diff_angle_z" + "timer_period", + "heading_velocity_maximum", + "heading_velocity_scale_factor_tolerance", + "angular_velocity_maximum", + "angular_velocity_scale_factor_tolerance", + "angular_velocity_bias_tolerance", + "pose_estimator_longitudinal_tolerance", + "pose_estimator_lateral_tolerance", + "pose_estimator_vertical_tolerance", + "pose_estimator_angular_tolerance" ] } }, diff --git a/localization/pose_instability_detector/src/pose_instability_detector.cpp b/localization/pose_instability_detector/src/pose_instability_detector.cpp index c2bce6a3db288..fa7b2ecf16562 100644 --- a/localization/pose_instability_detector/src/pose_instability_detector.cpp +++ b/localization/pose_instability_detector/src/pose_instability_detector.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" @@ -20,17 +20,32 @@ #include +#include +#include +#include #include PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & options) : rclcpp::Node("pose_instability_detector", options), - threshold_diff_position_x_(this->declare_parameter("threshold_diff_position_x")), - threshold_diff_position_y_(this->declare_parameter("threshold_diff_position_y")), - threshold_diff_position_z_(this->declare_parameter("threshold_diff_position_z")), - threshold_diff_angle_x_(this->declare_parameter("threshold_diff_angle_x")), - threshold_diff_angle_y_(this->declare_parameter("threshold_diff_angle_y")), - threshold_diff_angle_z_(this->declare_parameter("threshold_diff_angle_z")) + timer_period_(this->declare_parameter("timer_period")), + heading_velocity_maximum_(this->declare_parameter("heading_velocity_maximum")), + heading_velocity_scale_factor_tolerance_( + this->declare_parameter("heading_velocity_scale_factor_tolerance")), + angular_velocity_maximum_(this->declare_parameter("angular_velocity_maximum")), + angular_velocity_scale_factor_tolerance_( + this->declare_parameter("angular_velocity_scale_factor_tolerance")), + angular_velocity_bias_tolerance_( + this->declare_parameter("angular_velocity_bias_tolerance")), + pose_estimator_longitudinal_tolerance_( + this->declare_parameter("pose_estimator_longitudinal_tolerance")), + pose_estimator_lateral_tolerance_( + this->declare_parameter("pose_estimator_lateral_tolerance")), + pose_estimator_vertical_tolerance_( + this->declare_parameter("pose_estimator_vertical_tolerance")), + pose_estimator_angular_tolerance_( + this->declare_parameter("pose_estimator_angular_tolerance")) { + // Define subscribers, publishers and a timer. odometry_sub_ = this->create_subscription( "~/input/odometry", 10, std::bind(&PoseInstabilityDetector::callback_odometry, this, std::placeholders::_1)); @@ -39,9 +54,8 @@ PoseInstabilityDetector::PoseInstabilityDetector(const rclcpp::NodeOptions & opt "~/input/twist", 10, std::bind(&PoseInstabilityDetector::callback_twist, this, std::placeholders::_1)); - const double interval_sec = this->declare_parameter("interval_sec"); timer_ = rclcpp::create_timer( - this, this->get_clock(), std::chrono::duration(interval_sec), + this, this->get_clock(), std::chrono::duration(timer_period_), std::bind(&PoseInstabilityDetector::callback_timer, this)); diff_pose_pub_ = this->create_publisher("~/debug/diff_pose", 10); @@ -61,6 +75,7 @@ void PoseInstabilityDetector::callback_twist( void PoseInstabilityDetector::callback_timer() { + // odometry callback and timer callback has to be called at least once if (latest_odometry_ == std::nullopt) { return; } @@ -69,6 +84,16 @@ void PoseInstabilityDetector::callback_timer() return; } + // twist callback has to be called at least once + if (twist_buffer_.empty()) { + return; + } + + // time variables + const rclcpp::Time latest_odometry_time = rclcpp::Time(latest_odometry_->header.stamp); + const rclcpp::Time prev_odometry_time = rclcpp::Time(prev_odometry_->header.stamp); + + // define lambda function to convert quaternion to rpy auto quat_to_rpy = [](const Quaternion & quat) { tf2::Quaternion tf2_quat(quat.x, quat.y, quat.z, quat.w); tf2::Matrix3x3 mat(tf2_quat); @@ -79,70 +104,48 @@ void PoseInstabilityDetector::callback_timer() return std::make_tuple(roll, pitch, yaw); }; - Pose pose = prev_odometry_->pose.pose; - rclcpp::Time prev_time = rclcpp::Time(prev_odometry_->header.stamp); - for (const TwistWithCovarianceStamped & twist_with_cov : twist_buffer_) { - const Twist twist = twist_with_cov.twist.twist; - - const rclcpp::Time curr_time = rclcpp::Time(twist_with_cov.header.stamp); - if (curr_time > latest_odometry_->header.stamp) { + // delete twist data older than prev_odometry_ (but preserve the one right before prev_odometry_) + while (twist_buffer_.size() > 1) { + if (rclcpp::Time(twist_buffer_[1].header.stamp) < prev_odometry_time) { + twist_buffer_.pop_front(); + } else { break; } + } - const rclcpp::Duration time_diff = curr_time - prev_time; - const double time_diff_sec = time_diff.seconds(); - if (time_diff_sec < 0.0) { - continue; - } - - // quat to rpy - auto [ang_x, ang_y, ang_z] = quat_to_rpy(pose.orientation); - - // rpy update - ang_x += twist.angular.x * time_diff_sec; - ang_y += twist.angular.y * time_diff_sec; - ang_z += twist.angular.z * time_diff_sec; - tf2::Quaternion quat; - quat.setRPY(ang_x, ang_y, ang_z); + // dead reckoning from prev_odometry_ to latest_odometry_ + PoseStamped::SharedPtr prev_pose = std::make_shared(); + prev_pose->header = prev_odometry_->header; + prev_pose->pose = prev_odometry_->pose.pose; - // Convert twist to world frame - tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); - linear_velocity = tf2::quatRotate(quat, linear_velocity); - - // update - pose.position.x += linear_velocity.x() * time_diff_sec; - pose.position.y += linear_velocity.y() * time_diff_sec; - pose.position.z += linear_velocity.z() * time_diff_sec; - pose.orientation.x = quat.x(); - pose.orientation.y = quat.y(); - pose.orientation.z = quat.z(); - pose.orientation.w = quat.w(); - prev_time = curr_time; - } + Pose::SharedPtr DR_pose = std::make_shared(); + dead_reckon(prev_pose, latest_odometry_time, twist_buffer_, DR_pose); - // compare pose and latest_odometry_ + // compare dead reckoning pose and latest_odometry_ const Pose latest_ekf_pose = latest_odometry_->pose.pose; - const Pose ekf_to_odom = tier4_autoware_utils::inverseTransformPose(pose, latest_ekf_pose); - const geometry_msgs::msg::Point pos = ekf_to_odom.position; - const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_odom.orientation); + const Pose ekf_to_DR = tier4_autoware_utils::inverseTransformPose(*DR_pose, latest_ekf_pose); + const geometry_msgs::msg::Point pos = ekf_to_DR.position; + const auto [ang_x, ang_y, ang_z] = quat_to_rpy(ekf_to_DR.orientation); const std::vector values = {pos.x, pos.y, pos.z, ang_x, ang_y, ang_z}; - const rclcpp::Time stamp = latest_odometry_->header.stamp; - // publish diff_pose for debug PoseStamped diff_pose; - diff_pose.header = latest_odometry_->header; - diff_pose.pose = ekf_to_odom; + diff_pose.header.stamp = latest_odometry_time; + diff_pose.header.frame_id = "base_link"; + diff_pose.pose = ekf_to_DR; diff_pose_pub_->publish(diff_pose); - const std::vector thresholds = {threshold_diff_position_x_, threshold_diff_position_y_, - threshold_diff_position_z_, threshold_diff_angle_x_, - threshold_diff_angle_y_, threshold_diff_angle_z_}; + // publish diagnostics + ThresholdValues threshold_values = + calculate_threshold((latest_odometry_time - prev_odometry_time).seconds()); + + const std::vector thresholds = {threshold_values.position_x, threshold_values.position_y, + threshold_values.position_z, threshold_values.angle_x, + threshold_values.angle_y, threshold_values.angle_z}; const std::vector labels = {"diff_position_x", "diff_position_y", "diff_position_z", "diff_angle_x", "diff_angle_y", "diff_angle_z"}; - // publish diagnostics DiagnosticStatus status; status.name = "localization: pose_instability_detector"; status.hardware_id = this->get_name(); @@ -166,13 +169,239 @@ void PoseInstabilityDetector::callback_timer() status.message = (all_ok ? "OK" : "WARN"); DiagnosticArray diagnostics; - diagnostics.header.stamp = stamp; + diagnostics.header.stamp = latest_odometry_time; diagnostics.status.emplace_back(status); diagnostics_pub_->publish(diagnostics); // prepare for next loop prev_odometry_ = latest_odometry_; - twist_buffer_.clear(); +} + +PoseInstabilityDetector::ThresholdValues PoseInstabilityDetector::calculate_threshold( + double interval_sec) +{ + // Calculate maximum longitudinal difference + const double longitudinal_difference = + heading_velocity_maximum_ * heading_velocity_scale_factor_tolerance_ * 0.01 * interval_sec; + + // Calculate maximum lateral and vertical difference + double lateral_difference = 0.0; + + const std::vector heading_velocity_signs = {1.0, -1.0, -1.0, 1.0}; + const std::vector angular_velocity_signs = {1.0, 1.0, -1.0, -1.0}; + + const double nominal_variation_x = heading_velocity_maximum_ / angular_velocity_maximum_ * + sin(angular_velocity_maximum_ * interval_sec); + const double nominal_variation_y = heading_velocity_maximum_ / angular_velocity_maximum_ * + (1 - cos(angular_velocity_maximum_ * interval_sec)); + + for (int i = 0; i < 4; i++) { + const double edge_heading_velocity = + heading_velocity_maximum_ * + (1 + heading_velocity_signs[i] * heading_velocity_scale_factor_tolerance_ * 0.01); + const double edge_angular_velocity = + angular_velocity_maximum_ * + (1 + angular_velocity_signs[i] * angular_velocity_scale_factor_tolerance_ * 0.01) + + angular_velocity_signs[i] * angular_velocity_bias_tolerance_; + + const double edge_variation_x = + edge_heading_velocity / edge_angular_velocity * sin(edge_angular_velocity * interval_sec); + const double edge_variation_y = edge_heading_velocity / edge_angular_velocity * + (1 - cos(edge_angular_velocity * interval_sec)); + + const double diff_variation_x = edge_variation_x - nominal_variation_x; + const double diff_variation_y = edge_variation_y - nominal_variation_y; + + const double lateral_difference_candidate = abs( + diff_variation_x * sin(angular_velocity_maximum_ * interval_sec) - + diff_variation_y * cos(angular_velocity_maximum_ * interval_sec)); + lateral_difference = std::max(lateral_difference, lateral_difference_candidate); + } + + const double vertical_difference = lateral_difference; + + // Calculate maximum angular difference + const double roll_difference = + (angular_velocity_maximum_ * angular_velocity_scale_factor_tolerance_ * 0.01 + + angular_velocity_bias_tolerance_) * + interval_sec; + const double pitch_difference = roll_difference; + const double yaw_difference = roll_difference; + + // Set thresholds + ThresholdValues result_values; + result_values.position_x = longitudinal_difference + pose_estimator_longitudinal_tolerance_; + result_values.position_y = lateral_difference + pose_estimator_lateral_tolerance_; + result_values.position_z = vertical_difference + pose_estimator_vertical_tolerance_; + result_values.angle_x = roll_difference + pose_estimator_angular_tolerance_; + result_values.angle_y = pitch_difference + pose_estimator_angular_tolerance_; + result_values.angle_z = yaw_difference + pose_estimator_angular_tolerance_; + + return result_values; +} + +void PoseInstabilityDetector::dead_reckon( + PoseStamped::SharedPtr & initial_pose, const rclcpp::Time & end_time, + const std::deque & twist_deque, Pose::SharedPtr & estimated_pose) +{ + // get start time + rclcpp::Time start_time = rclcpp::Time(initial_pose->header.stamp); + + // initialize estimated_pose + estimated_pose->position = initial_pose->pose.position; + estimated_pose->orientation = initial_pose->pose.orientation; + + // cut out necessary twist data + std::deque sliced_twist_deque = + clip_out_necessary_twist(twist_deque, start_time, end_time); + + // dead reckoning + rclcpp::Time prev_odometry_time = rclcpp::Time(sliced_twist_deque.front().header.stamp); + tf2::Quaternion prev_orientation; + tf2::fromMsg(estimated_pose->orientation, prev_orientation); + + for (size_t i = 1; i < sliced_twist_deque.size(); ++i) { + const rclcpp::Time curr_time = rclcpp::Time(sliced_twist_deque[i].header.stamp); + const double time_diff_sec = (curr_time - prev_odometry_time).seconds(); + + const Twist twist = sliced_twist_deque[i - 1].twist.twist; + + // variation of orientation (rpy update) + tf2::Quaternion delta_orientation; + tf2::Vector3 rotation_axis(twist.angular.x, twist.angular.y, twist.angular.z); + if (rotation_axis.length() > 0.0) { + delta_orientation.setRotation( + rotation_axis.normalized(), rotation_axis.length() * time_diff_sec); + } else { + delta_orientation.setValue(0.0, 0.0, 0.0, 1.0); + } + + tf2::Quaternion curr_orientation; + curr_orientation = prev_orientation * delta_orientation; + curr_orientation.normalize(); + + // average quaternion of two frames + tf2::Quaternion average_quat = prev_orientation.slerp(curr_orientation, 0.5); + + // Convert twist to world frame (take average of two frames) + tf2::Vector3 linear_velocity(twist.linear.x, twist.linear.y, twist.linear.z); + linear_velocity = tf2::quatRotate(average_quat, linear_velocity); + + // xyz update + estimated_pose->position.x += linear_velocity.x() * time_diff_sec; + estimated_pose->position.y += linear_velocity.y() * time_diff_sec; + estimated_pose->position.z += linear_velocity.z() * time_diff_sec; + + // update previous variables + prev_odometry_time = curr_time; + prev_orientation = curr_orientation; + } + estimated_pose->orientation.x = prev_orientation.x(); + estimated_pose->orientation.y = prev_orientation.y(); + estimated_pose->orientation.z = prev_orientation.z(); + estimated_pose->orientation.w = prev_orientation.w(); +} + +std::deque +PoseInstabilityDetector::clip_out_necessary_twist( + const std::deque & twist_buffer, const rclcpp::Time & start_time, + const rclcpp::Time & end_time) +{ + // If there is only one element in the twist_buffer, return a deque that has the same twist + // from the start till the end + if (twist_buffer.size() == 1) { + TwistWithCovarianceStamped twist = twist_buffer.front(); + std::deque simple_twist_deque; + + twist.header.stamp = start_time; + simple_twist_deque.push_back(twist); + + twist.header.stamp = end_time; + simple_twist_deque.push_back(twist); + + return simple_twist_deque; + } + + // get iterator to the element that is right before start_time (if it does not exist, start_it = + // twist_buffer.begin()) + auto start_it = twist_buffer.begin(); + + for (auto it = twist_buffer.begin(); it != twist_buffer.end(); ++it) { + if (rclcpp::Time(it->header.stamp) > start_time) { + break; + } + start_it = it; + } + + // get iterator to the element that is right after end_time (if it does not exist, end_it = + // twist_buffer.end()) + auto end_it = twist_buffer.end(); + end_it--; + for (auto it = end_it; it != twist_buffer.begin(); --it) { + if (rclcpp::Time(it->header.stamp) < end_time) { + break; + } + end_it = it; + } + + // Create result deque + std::deque result_deque(start_it, end_it); + + // If the first element is later than start_time, add the first element to the front of the + // result_deque + if (rclcpp::Time(result_deque.front().header.stamp) > start_time) { + TwistWithCovarianceStamped start_twist = *start_it; + start_twist.header.stamp = start_time; + result_deque.push_front(start_twist); + } else { + // If the first element is earlier than start_time, interpolate the first element + rclcpp::Time time0 = rclcpp::Time(result_deque[0].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[1].header.stamp); + double ratio = (start_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[0].twist.twist; + Twist twist1 = result_deque[1].twist.twist; + result_deque[0].twist.twist.linear.x = twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[0].twist.twist.linear.y = twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[0].twist.twist.linear.z = twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[0].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[0].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[0].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[0].header.stamp = start_time; + } + + // If the last element is earlier than end_time, add the last element to the back of the + // result_deque + if (rclcpp::Time(result_deque.back().header.stamp) < end_time) { + TwistWithCovarianceStamped end_twist = *end_it; + end_twist.header.stamp = end_time; + result_deque.push_back(end_twist); + } else { + // If the last element is later than end_time, interpolate the last element + rclcpp::Time time0 = rclcpp::Time(result_deque[result_deque.size() - 2].header.stamp); + rclcpp::Time time1 = rclcpp::Time(result_deque[result_deque.size() - 1].header.stamp); + double ratio = (end_time - time0).seconds() / (time1 - time0).seconds(); + Twist twist0 = result_deque[result_deque.size() - 2].twist.twist; + Twist twist1 = result_deque[result_deque.size() - 1].twist.twist; + result_deque[result_deque.size() - 1].twist.twist.linear.x = + twist1.linear.x * ratio + twist0.linear.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.y = + twist1.linear.y * ratio + twist0.linear.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.linear.z = + twist1.linear.z * ratio + twist0.linear.z * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.x = + twist1.angular.x * ratio + twist0.angular.x * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.y = + twist1.angular.y * ratio + twist0.angular.y * (1 - ratio); + result_deque[result_deque.size() - 1].twist.twist.angular.z = + twist1.angular.z * ratio + twist0.angular.z * (1 - ratio); + + result_deque[result_deque.size() - 1].header.stamp = end_time; + } + return result_deque; } #include diff --git a/localization/pose_instability_detector/test/test.cpp b/localization/pose_instability_detector/test/test.cpp index 5ea03859d7731..9300984967d4b 100644 --- a/localization/pose_instability_detector/test/test.cpp +++ b/localization/pose_instability_detector/test/test.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "../src/pose_instability_detector.hpp" +#include "autoware/pose_instability_detector/pose_instability_detector.hpp" #include "test_message_helper_node.hpp" #include @@ -81,6 +81,11 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -88,11 +93,6 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 2.0, 0.0, 0.0); - // send the twist message2 (move 1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -101,7 +101,9 @@ TEST_F(TestPoseInstabilityDetector, output_ok_when_twist_matches_odometry) // N // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; @@ -124,6 +126,11 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL timestamp.nanosec = 0; helper_->send_odometry_message(timestamp, 10.0, 0.0, 0.0); + // send the twist message1 (move 0.1m in x direction) + timestamp.sec = 0; + timestamp.nanosec = 5e8; + helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); + // process the above message (by timer_callback) helper_->received_diagnostic_array_flag = false; while (!helper_->received_diagnostic_array_flag) { @@ -131,11 +138,6 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - // send the twist message1 (move 0.1m in x direction) - timestamp.sec = 0; - timestamp.nanosec = 5e8; - helper_->send_twist_message(timestamp, 0.2, 0.0, 0.0); - // send the twist message2 (move 0.1m in x direction) timestamp.sec = 1; timestamp.nanosec = 0; @@ -144,7 +146,9 @@ TEST_F(TestPoseInstabilityDetector, output_warn_when_twist_is_too_small) // NOL // send the second odometry message (finish x = 12) timestamp.sec = 2; timestamp.nanosec = 0; - helper_->send_odometry_message(timestamp, 12.0, 0.0, 0.0); + helper_->send_odometry_message(timestamp, 14.0, 0.0, 0.0); + + executor_.spin_some(); // process the above messages (by timer_callback) helper_->received_diagnostic_array_flag = false; diff --git a/planning/.pages b/planning/.pages index 7b932118b5010..6e3c6d5d49104 100644 --- a/planning/.pages +++ b/planning/.pages @@ -87,4 +87,4 @@ nav: - 'Planning Test Utils': planning/planning_test_utils - 'Planning Test Manager': planning/autoware_planning_test_manager - 'Planning Topic Converter': planning/autoware_planning_topic_converter - - 'Planning Validator': planning/planning_validator + - 'Planning Validator': planning/autoware_planning_validator diff --git a/planning/autoware_behavior_path_start_planner_module/README.md b/planning/autoware_behavior_path_start_planner_module/README.md index 71aa0a1abe704..bcb3b1bec1fd9 100644 --- a/planning/autoware_behavior_path_start_planner_module/README.md +++ b/planning/autoware_behavior_path_start_planner_module/README.md @@ -526,7 +526,7 @@ If a safe path cannot be generated from the current position, search backwards f ### **freespace pull out** If the vehicle gets stuck with pull out along lanes, execute freespace pull out. -To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` +To run this feature, you need to set `parking_lot` to the map, `activate_by_scenario` of [costmap_generator](../autoware_costmap_generator/README.md) to `false` and `enable_freespace_planner` to `true` diff --git a/planning/autoware_behavior_velocity_intersection_module/package.xml b/planning/autoware_behavior_velocity_intersection_module/package.xml index 204a267594c03..401e53586c649 100644 --- a/planning/autoware_behavior_velocity_intersection_module/package.xml +++ b/planning/autoware_behavior_velocity_intersection_module/package.xml @@ -18,9 +18,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common fmt geometry_msgs interpolation diff --git a/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp index 252d7c2a9f61e..ab09ca2e6e746 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include +#include +#include #include #include @@ -45,7 +45,7 @@ visualization_msgs::msg::MarkerArray createLaneletPolygonsMarkerArray( visualization_msgs::msg::MarkerArray msg; int32_t i = 0; - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(lane_id); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(lane_id); for (const auto & polygon : polygons) { visualization_msgs::msg::Marker marker{}; marker.header.frame_id = "map"; @@ -224,7 +224,7 @@ constexpr std::tuple light_blue() } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -472,7 +472,7 @@ visualization_msgs::msg::MarkerArray MergeFromPrivateRoadModule::createDebugMark const auto state = state_machine_.getState(); - int32_t uid = behavior_velocity_planner::planning_utils::bitShift(module_id_); + int32_t uid = autoware::behavior_velocity_planner::planning_utils::bitShift(module_id_); const auto now = this->clock_->now(); if (state == StateMachine::State::STOP) { appendMarkerArray( @@ -496,4 +496,4 @@ motion_utils::VirtualWalls MergeFromPrivateRoadModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp index 0dc0e3aab4b87..5d1f1a1fcfca2 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.cpp @@ -14,7 +14,7 @@ #include "decision_result.hpp" -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { std::string formatDecisionResult(const DecisionResult & decision_result) { @@ -65,4 +65,4 @@ std::string formatDecisionResult(const DecisionResult & decision_result) return ""; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp index bf0b5520f7a32..26f78616a7b61 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/decision_result.hpp @@ -19,7 +19,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** @@ -172,6 +172,6 @@ using DecisionResult = std::variant< std::string formatDecisionResult(const DecisionResult & decision_result); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // DECISION_RESULT_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp index c855164e24d5b..d8bada002e37b 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/interpolated_path_info.hpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** @@ -43,6 +43,6 @@ struct InterpolatedPathInfo std::optional> lane_id_interval{std::nullopt}; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // INTERPOLATED_PATH_INFO_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp index bf833ee07c155..be7e9e25cc8bc 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.cpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { void IntersectionLanelets::update( @@ -79,4 +79,4 @@ void IntersectionLanelets::update( } } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp index 3682787237d42..a7ca5eb3b0bc1 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_lanelets.hpp @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** @@ -190,6 +190,6 @@ struct PathLanelets // conflicting lanelets plus the next lane part of the // path }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_LANELETS_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp index 85876ffc98380..98325f0aa8735 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/intersection_stoplines.hpp @@ -17,7 +17,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** @@ -72,6 +72,6 @@ struct IntersectionStopLines */ size_t occlusion_wo_tl_pass_judge_line{0}; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // INTERSECTION_STOPLINES_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp index ac32ad553d2b7..8f540603791cb 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -589,11 +589,12 @@ bool MergeFromPrivateModuleManager::hasSameParentLaneletAndTurnDirectionWithRegi return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::IntersectionModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::IntersectionModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::MergeFromPrivateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp index 7bbc8d51bbe9e..ecf6bb810f313 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "scene_intersection.hpp" #include "scene_merge_from_private_road.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -90,6 +90,6 @@ class MergeFromPrivateModulePlugin : public PluginWrapper findPassageInterval( lane_position, path, {enter_idx, exit_idx}, {object_enter_time, object_exit_time}}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp index 3d1113656c3e3..0ba9947fdb318 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/object_manager.hpp @@ -52,7 +52,7 @@ struct hash }; } // namespace std -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** @@ -287,6 +287,6 @@ std::optional findPassageInterval( const std::optional & first_attention_lane_opt, const std::optional & second_attention_lane_opt); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OBJECT_MANAGER_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/result.hpp b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp index da89920e59a85..1a833b268a5d8 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/result.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/result.hpp @@ -18,7 +18,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ Result make_err(Args &&... args) return Result(Error{std::forward(args)...}); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // RESULT_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp index 8e50c140894e7..beed0aab8e382 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include // for toGeomPoly -#include +#include // for toGeomPoly +#include #include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -1412,4 +1412,4 @@ IntersectionModule::PassJudgeStatus IntersectionModule::isOverPassJudgeLinesStat safely_passed_1st_judge_line_first_time, safely_passed_2nd_judge_line_first_time}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp index b1cfcdee04fa6..e25f41a18028a 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -22,8 +22,8 @@ #include "object_manager.hpp" #include "result.hpp" -#include -#include +#include +#include #include #include @@ -43,7 +43,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class IntersectionModule : public SceneModuleInterface @@ -818,6 +818,6 @@ class IntersectionModule : public SceneModuleInterface rclcpp::Publisher::SharedPtr object_ttc_pub_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_INTERSECTION_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp index 48387bc6aa398..fb06ed9d811c8 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_collision.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly -#include // for smoothPath +#include // for toGeomPoly +#include // for smoothPath #include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -964,4 +964,4 @@ IntersectionModule::TimeDistanceArray IntersectionModule::calcIntersectionPassin return time_distance_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp index 0fb6042e5aadb..c43f8617897b9 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_occlusion.cpp @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -449,4 +449,4 @@ IntersectionModule::OcclusionType IntersectionModule::detectOcclusion( debug_data_.static_occlusion = true; return StaticallyOccluded{min_dist}; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp index d4466c1e9431b..223e8eca84fe8 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_prepare_data.cpp @@ -15,8 +15,8 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for to_bg2d -#include // for planning_utils:: +#include // for to_bg2d +#include // for planning_utils:: #include #include // for lanelet::autoware::RoadMarking #include @@ -106,7 +106,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -122,7 +122,7 @@ std::optional> getFirstPoi const auto & p = path.points.at(i).point.pose.position; for (const auto & polygon : polygons) { const auto polygon_2d = lanelet::utils::to2D(polygon); - is_in_lanelet = bg::within(behavior_velocity_planner::to_bg2d(p), polygon_2d); + is_in_lanelet = bg::within(autoware::behavior_velocity_planner::to_bg2d(p), polygon_2d); if (is_in_lanelet) { return std::make_optional>( i, polygon); @@ -157,7 +157,7 @@ double getHighestCurvature(const lanelet::ConstLineString3d & centerline) } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -907,4 +907,4 @@ std::vector IntersectionModule::generateDetectionLan return detection_divisions; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp index c7de1805327c3..eecb02d07c386 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_intersection_stuck.cpp @@ -15,7 +15,7 @@ #include "scene_intersection.hpp" #include "util.hpp" -#include // for toGeomPoly +#include // for toGeomPoly #include #include @@ -114,7 +114,7 @@ lanelet::ConstLanelet createLaneletFromArcLength( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -422,4 +422,4 @@ bool IntersectionModule::checkYieldStuckVehicleInIntersection( } return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index be8d94c3b306d..1aaed779e1b79 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -16,8 +16,8 @@ #include "util.hpp" -#include -#include +#include +#include #include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -206,4 +206,4 @@ lanelet::ConstLanelets MergeFromPrivateRoadModule::getAttentionLanelets() const return attention_lanelets; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 0b783cf2a7ebd..19e9ea44869ea 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -15,8 +15,8 @@ #ifndef SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ #define SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ -#include -#include +#include +#include #include #include @@ -35,7 +35,7 @@ * lanes before entering intersection */ -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class MergeFromPrivateRoadModule : public SceneModuleInterface { @@ -88,6 +88,6 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_MERGE_FROM_PRIVATE_ROAD_HPP_ diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.cpp b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp index a083185786e42..09f16bcada3c1 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/util.cpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.cpp @@ -16,9 +16,9 @@ #include "interpolated_path_info.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { namespace bg = boost::geometry; @@ -399,4 +399,4 @@ std::vector getPolygon3dFromLanelets( return polys; } -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util diff --git a/planning/autoware_behavior_velocity_intersection_module/src/util.hpp b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp index 27200e663503d..ef826380afa69 100644 --- a/planning/autoware_behavior_velocity_intersection_module/src/util.hpp +++ b/planning/autoware_behavior_velocity_intersection_module/src/util.hpp @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::util +namespace autoware::behavior_velocity_planner::util { /** @@ -141,6 +141,6 @@ getFirstPointInsidePolygonsByFootprint( std::vector getPolygon3dFromLanelets( const lanelet::ConstLanelets & ll_vec); -} // namespace behavior_velocity_planner::util +} // namespace autoware::behavior_velocity_planner::util #endif // UTIL_HPP_ diff --git a/planning/autoware_behavior_velocity_planner/package.xml b/planning/autoware_behavior_velocity_planner/package.xml index c90d4d01cbca5..4d5dc17264df0 100644 --- a/planning/autoware_behavior_velocity_planner/package.xml +++ b/planning/autoware_behavior_velocity_planner/package.xml @@ -34,11 +34,11 @@ rosidl_default_generators + autoware_behavior_velocity_planner_common autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common diagnostic_msgs eigen geometry_msgs diff --git a/planning/autoware_behavior_velocity_planner/src/node.cpp b/planning/autoware_behavior_velocity_planner/src/node.cpp index ffbc4ef9174dc..3ff4e813b3266 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.cpp +++ b/planning/autoware_behavior_velocity_planner/src/node.cpp @@ -14,8 +14,8 @@ #include "node.hpp" +#include #include -#include #include #include #include @@ -310,7 +310,7 @@ void BehaviorVelocityPlannerNode::onParam() // constructed. It would be required if it was a callback. std::lock_guard // lock(mutex_); planner_data_.velocity_smoother_ = - std::make_unique(*this); + std::make_unique(*this); planner_data_.velocity_smoother_->setWheelBase(planner_data_.vehicle_info_.wheel_base_m); } @@ -439,14 +439,14 @@ autoware_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath( // screening const auto filtered_path = - ::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); + autoware::behavior_velocity_planner::filterLitterPathPoint(to_path(velocity_planned_path)); // interpolation - const auto interpolated_path_msg = ::behavior_velocity_planner::interpolatePath( + const auto interpolated_path_msg = autoware::behavior_velocity_planner::interpolatePath( filtered_path, forward_path_length_, behavior_output_path_interval_); // check stop point - output_path_msg = ::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); + output_path_msg = autoware::behavior_velocity_planner::filterStopPathPoint(interpolated_path_msg); output_path_msg.header.frame_id = "map"; output_path_msg.header.stamp = this->now(); diff --git a/planning/autoware_behavior_velocity_planner/src/node.hpp b/planning/autoware_behavior_velocity_planner/src/node.hpp index 62ceef5f04ea6..b2157bfb818ce 100644 --- a/planning/autoware_behavior_velocity_planner/src/node.hpp +++ b/planning/autoware_behavior_velocity_planner/src/node.hpp @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include @@ -49,7 +49,6 @@ namespace autoware::behavior_velocity_planner using autoware_behavior_velocity_planner::srv::LoadPlugin; using autoware_behavior_velocity_planner::srv::UnloadPlugin; using autoware_map_msgs::msg::LaneletMapBin; -using ::behavior_velocity_planner::TrafficSignalStamped; using tier4_planning_msgs::msg::VelocityLimit; class BehaviorVelocityPlannerNode : public rclcpp::Node diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp index f462fc963f17b..93209a10180be 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.cpp @@ -50,7 +50,8 @@ diagnostic_msgs::msg::DiagnosticStatus makeStopReasonDiag( } // namespace BehaviorVelocityPlannerManager::BehaviorVelocityPlannerManager() -: plugin_loader_("autoware_behavior_velocity_planner", "behavior_velocity_planner::PluginInterface") +: plugin_loader_( + "autoware_behavior_velocity_planner", "autoware::behavior_velocity_planner::PluginInterface") { } diff --git a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp index 9e7f2942bb067..73193a002918d 100644 --- a/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp +++ b/planning/autoware_behavior_velocity_planner/src/planner_manager.hpp @@ -15,8 +15,8 @@ #ifndef PLANNER_MANAGER_HPP_ #define PLANNER_MANAGER_HPP_ -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlannerData; -using ::behavior_velocity_planner::PluginInterface; class BehaviorVelocityPlannerManager { 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 2e971ed238751..cdff8af4ac56e 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 @@ -68,21 +68,21 @@ std::shared_ptr generateNode() }; std::vector module_names; - module_names.emplace_back("behavior_velocity_planner::CrosswalkModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::CrosswalkModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::WalkwayModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::TrafficLightModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::TrafficLightModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::IntersectionModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::MergeFromPrivateModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::BlindSpotModulePlugin"); - module_names.emplace_back("behavior_velocity_planner::DetectionAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::MergeFromPrivateModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::BlindSpotModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::DetectionAreaModulePlugin"); module_names.emplace_back("autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin"); - 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("autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::StopLineModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::OcclusionSpotModulePlugin"); 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"); + module_names.emplace_back("autoware::behavior_velocity_planner::SpeedBumpModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::OutOfLaneModulePlugin"); + module_names.emplace_back("autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin"); std::vector params; params.emplace_back("launch_modules", module_names); diff --git a/planning/behavior_velocity_planner_common/CMakeLists.txt b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt similarity index 93% rename from planning/behavior_velocity_planner_common/CMakeLists.txt rename to planning/autoware_behavior_velocity_planner_common/CMakeLists.txt index c8847164851e8..9cb992312f52a 100644 --- a/planning/behavior_velocity_planner_common/CMakeLists.txt +++ b/planning/autoware_behavior_velocity_planner_common/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(behavior_velocity_planner_common) +project(autoware_behavior_velocity_planner_common) find_package(autoware_cmake REQUIRED) autoware_package() diff --git a/planning/behavior_velocity_planner_common/README.md b/planning/autoware_behavior_velocity_planner_common/README.md similarity index 100% rename from planning/behavior_velocity_planner_common/README.md rename to planning/autoware_behavior_velocity_planner_common/README.md diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp similarity index 92% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp index 51511b94f3e33..c69f23215a369 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/planner_data.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ #include "route_handler/route_handler.hpp" +#include #include -#include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BehaviorVelocityPlannerNode; struct PlannerData @@ -88,7 +88,7 @@ struct PlannerData bool is_simulation = false; // velocity smoother - std::shared_ptr velocity_smoother_; + std::shared_ptr velocity_smoother_; // route handler std::shared_ptr route_handler_; // parameters @@ -148,6 +148,6 @@ struct PlannerData return std::make_optional(traffic_light_id_map.at(id)); } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLANNER_DATA_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp similarity index 75% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp index dcdb4a7052cc0..86579f06790b2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_interface.hpp @@ -12,17 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class PluginInterface @@ -38,6 +38,6 @@ class PluginInterface virtual const char * getModuleName() = 0; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp index abb14dd8b2356..e82211937e55b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/plugin_wrapper.hpp @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { template @@ -48,6 +48,6 @@ class PluginWrapper : public PluginInterface std::unique_ptr scene_manager_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__PLUGIN_WRAPPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp similarity index 95% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp index 3e7992207f3f1..53b6e064d73b4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/scene_module_interface.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ -#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using builtin_interfaces::msg::Time; @@ -276,6 +276,6 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface } }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp similarity index 94% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp index c9d292536ac13..5ea1ae9fffcc1 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ -#include +#include #include #include @@ -27,7 +27,7 @@ #define EIGEN_MPL2_ONLY #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -195,6 +195,6 @@ std::optional createTargetPoint( const size_t lane_id, const double margin, const double vehicle_offset); } // namespace arc_lane_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__ARC_LANE_UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp similarity index 88% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp index bf238ecad55cb..22bba2b90dcc2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ #include @@ -46,7 +46,7 @@ BOOST_GEOMETRY_REGISTER_POINT_3D( autoware_planning_msgs::msg::TrajectoryPoint, double, cs::cartesian, pose.position.x, pose.position.y, pose.position.z) -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -77,6 +77,6 @@ Polygon2d upScalePolygon( geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__BOOST_GEOMETRY_HELPER_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp similarity index 86% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp index ab44af265fbaa..c6e5d45ec4eb6 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/debug.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/debug.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -47,5 +47,5 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( const int64_t module_id, const rclcpp::Time & now, const double x, const double y, const double z, const double r, const double g, const double b); } // namespace debug -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp similarity index 78% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp index 55a82db1ae390..e9bccc42e1dc4 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/path_utilization.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/path_utilization.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ #include @@ -22,7 +22,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -33,6 +33,6 @@ autoware_planning_msgs::msg::Path filterLitterPathPoint( const autoware_planning_msgs::msg::Path & path); autoware_planning_msgs::msg::Path filterStopPathPoint( const autoware_planning_msgs::msg::Path & path); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__PATH_UTILIZATION_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp similarity index 87% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp index 73b0fa7d553a3..12dd4db930745 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/state_machine.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/state_machine.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief Manage stop-go states with safety margin time. @@ -92,5 +92,5 @@ class StateMachine std::shared_ptr start_time_; //! first time received GO when STOP state }; -} // namespace behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ +} // namespace autoware::behavior_velocity_planner +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__STATE_MACHINE_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp similarity index 76% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp index 2aadb7883a857..c5040f055c243 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/trajectory_utils.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/trajectory_utils.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ -#include +#include #include #include @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -40,6 +40,6 @@ bool smoothPath( const PathWithLaneId & in_path, PathWithLaneId & out_path, const std::shared_ptr & planner_data); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__TRAJECTORY_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp similarity index 93% rename from planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp rename to planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp index 4ef4bb91a295d..2fe728847b8f2 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/autoware_behavior_velocity_planner_common/include/autoware_behavior_velocity_planner_common/utilization/util.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ -#define BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#ifndef AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#define AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ #include @@ -35,7 +35,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { struct DetectionRange { @@ -155,11 +155,13 @@ std::unordered_map, lanelet::ConstLanelet> get std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -239,6 +241,6 @@ lanelet::ConstLanelets getConstLaneletsFromIds( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner -#endif // BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ +#endif // AUTOWARE_BEHAVIOR_VELOCITY_PLANNER_COMMON__UTILIZATION__UTIL_HPP_ diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/autoware_behavior_velocity_planner_common/package.xml similarity index 93% rename from planning/behavior_velocity_planner_common/package.xml rename to planning/autoware_behavior_velocity_planner_common/package.xml index ba847d8b1f853..524387034e4b6 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/autoware_behavior_velocity_planner_common/package.xml @@ -1,9 +1,9 @@ - behavior_velocity_planner_common + autoware_behavior_velocity_planner_common 0.1.0 - The behavior_velocity_planner_common package + The autoware_behavior_velocity_planner_common package Tomoya Kimura Shumpei Wakabayashi diff --git a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/scene_module_interface.cpp rename to planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp index 3092d33418c8b..94cef496f4f75 100644 --- a/planning/behavior_velocity_planner_common/src/scene_module_interface.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/scene_module_interface.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include #include @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::StopWatch; @@ -281,4 +281,4 @@ void SceneModuleManagerInterfaceWithRTC::deleteExpiredModules( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp index e5705b1367e0e..d7e88f5f33901 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/arc_lane_util.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include @@ -62,7 +62,7 @@ return p * v; */ } // namespace -namespace behavior_velocity_planner::arc_lane_utils +namespace autoware::behavior_velocity_planner::arc_lane_utils { double calcSignedDistance(const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2) @@ -134,4 +134,4 @@ std::optional createTargetPoint( const auto front_idx = offset_segment->first; return std::make_pair(front_idx, target_pose); } -} // namespace behavior_velocity_planner::arc_lane_utils +} // namespace autoware::behavior_velocity_planner::arc_lane_utils diff --git a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp index 903cf5aab80e8..50f7cd269a904 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/boost_geometry_helper.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { Polygon2d lines2polygon(const LineString2d & left_line, const LineString2d & right_line) @@ -63,4 +63,4 @@ geometry_msgs::msg::Polygon toGeomPoly(const Polygon2d & polygon) } return polygon_msg; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp similarity index 94% rename from planning/behavior_velocity_planner_common/src/utilization/debug.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp index 00d746c56db85..a8e6828a048ae 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/debug.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/debug.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace debug { @@ -124,4 +124,4 @@ visualization_msgs::msg::MarkerArray createPointsMarkerArray( return msg; } } // namespace debug -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp similarity index 96% rename from planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp index fe956e9be9512..eb17242c06b1e 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/path_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/path_utilization.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include #include @@ -23,7 +23,7 @@ constexpr double DOUBLE_EPSILON = 1e-6; -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool splineInterpolate( const tier4_planning_msgs::msg::PathWithLaneId & input, const double interval, @@ -168,4 +168,4 @@ autoware_planning_msgs::msg::Path filterStopPathPoint( } return filtered_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp similarity index 91% rename from planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp index ecc314bca2009..1dc448bea5b86 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -// #include +// #include #include "motion_utils/trajectory/conversion.hpp" +#include #include -#include #include #include @@ -33,7 +33,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -82,11 +82,11 @@ bool smoothPath( traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } out_path = motion_utils::convertToPathWithLaneId(traj_smoothed); return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/src/utilization/util.cpp b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp similarity index 97% rename from planning/behavior_velocity_planner_common/src/utilization/util.cpp rename to planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp index b724d01346f1e..4578676aeeee7 100644 --- a/planning/behavior_velocity_planner_common/src/utilization/util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/src/utilization/util.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include #include @@ -89,7 +89,7 @@ geometry_msgs::msg::Pose transformRelCoordinate2D( } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace planning_utils { @@ -545,11 +545,13 @@ std::vector getLaneletsOnPath( std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } std::vector lanelets; @@ -692,4 +694,4 @@ std::set getAssociativeIntersectionLanelets( } } // namespace planning_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp similarity index 93% rename from planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp index fb2418ba9c829..b835aa0034ec4 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_arc_lane_util.cpp @@ -14,17 +14,18 @@ #include "utils.hpp" -#include -#include +#include +#include #include #include -using PathIndexWithPoint2d = behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; -using LineString2d = behavior_velocity_planner::LineString2d; -using Point2d = behavior_velocity_planner::Point2d; -namespace arc_lane_utils = behavior_velocity_planner::arc_lane_utils; +using PathIndexWithPoint2d = + autoware::behavior_velocity_planner::arc_lane_utils::PathIndexWithPoint2d; +using LineString2d = autoware::behavior_velocity_planner::LineString2d; +using Point2d = autoware::behavior_velocity_planner::Point2d; +namespace arc_lane_utils = autoware::behavior_velocity_planner::arc_lane_utils; namespace { @@ -121,7 +122,7 @@ TEST(findOffsetSegment, case_backward_offset_segment) TEST(checkCollision, various_cases) { - using behavior_velocity_planner::arc_lane_utils::checkCollision; + using autoware::behavior_velocity_planner::arc_lane_utils::checkCollision; constexpr double epsilon = 1e-6; { // normal case with collision diff --git a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp index ac7c6655ced87..fe6a1e00496b1 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_state_machine.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_state_machine.cpp @@ -14,7 +14,7 @@ #include "utils.hpp" -#include +#include #include #include @@ -23,8 +23,8 @@ #include #include -using StateMachine = behavior_velocity_planner::StateMachine; -using State = behavior_velocity_planner::StateMachine::State; +using StateMachine = autoware::behavior_velocity_planner::StateMachine; +using State = autoware::behavior_velocity_planner::StateMachine::State; int enumToInt(State s) { diff --git a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp similarity index 92% rename from planning/behavior_velocity_planner_common/test/src/test_utilization.cpp rename to planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp index fbc5f5d709c5c..5e6c92b662820 100644 --- a/planning/behavior_velocity_planner_common/test/src/test_utilization.cpp +++ b/planning/autoware_behavior_velocity_planner_common/test/src/test_utilization.cpp @@ -15,9 +15,9 @@ #include "motion_utils/trajectory/trajectory.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include @@ -35,7 +35,7 @@ TEST(is_ahead_of, nominal) { - using behavior_velocity_planner::planning_utils::isAheadOf; + using autoware::behavior_velocity_planner::planning_utils::isAheadOf; geometry_msgs::msg::Pose target = test::generatePose(0); geometry_msgs::msg::Pose origin = test::generatePose(1); bool is_ahead = isAheadOf(target, origin); @@ -47,7 +47,8 @@ TEST(is_ahead_of, nominal) TEST(smoothDeceleration, calculateMaxSlowDownVelocity) { - using behavior_velocity_planner::planning_utils::calcDecelerationVelocityFromDistanceToTarget; + using autoware::behavior_velocity_planner::planning_utils:: + calcDecelerationVelocityFromDistanceToTarget; const double current_accel = 1.0; const double current_velocity = 5.0; const double max_slow_down_jerk = -1.0; @@ -82,9 +83,9 @@ TEST(smoothDeceleration, calculateMaxSlowDownVelocity) TEST(specialInterpolation, specialInterpolation) { + using autoware::behavior_velocity_planner::interpolatePath; using autoware_planning_msgs::msg::Path; using autoware_planning_msgs::msg::PathPoint; - using behavior_velocity_planner::interpolatePath; using motion_utils::calcSignedArcLength; using motion_utils::searchZeroVelocityIndex; diff --git a/planning/behavior_velocity_planner_common/test/src/utils.hpp b/planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp similarity index 100% rename from planning/behavior_velocity_planner_common/test/src/utils.hpp rename to planning/autoware_behavior_velocity_planner_common/test/src/utils.hpp diff --git a/planning/autoware_behavior_velocity_run_out_module/package.xml b/planning/autoware_behavior_velocity_run_out_module/package.xml index cb2c2df58ffe2..330880b77d3da 100644 --- a/planning/autoware_behavior_velocity_run_out_module/package.xml +++ b/planning/autoware_behavior_velocity_run_out_module/package.xml @@ -19,10 +19,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common eigen geometry_msgs libboost-dev diff --git a/planning/autoware_behavior_velocity_run_out_module/plugins.xml b/planning/autoware_behavior_velocity_run_out_module/plugins.xml index 1ddcaf8e620e1..0fbaf5091d0f7 100644 --- a/planning/autoware_behavior_velocity_run_out_module/plugins.xml +++ b/planning/autoware_behavior_velocity_run_out_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp index 3c6d475950e7c..c38dd63fadba9 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/debug.hpp @@ -26,7 +26,6 @@ #include 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; diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp index d204c7ec6f2fc..5e8b0382a07c0 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.cpp @@ -32,8 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::Point2d; -using ::behavior_velocity_planner::splineInterpolate; namespace { // create quaternion facing to the nearest trajectory point diff --git a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp index b7ed815829a2b..8fb79fb9d8eaa 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/dynamic_obstacle.hpp @@ -18,9 +18,9 @@ #include "debug.hpp" #include "utils.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -47,7 +47,6 @@ 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; @@ -56,7 +55,6 @@ 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 diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp index 5ee50863fd162..002a49abae611 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.cpp @@ -22,7 +22,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_autoware_utils::getOrDeclareParameter; RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -203,4 +202,4 @@ void RunOutModuleManager::setDynamicObstacleCreator( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::RunOutModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp index f0c49b99c99e5..8cdb17e21d74b 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/manager.hpp @@ -17,17 +17,14 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include 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: diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp index 351ed8e7a09b8..e492bc99be615 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.cpp @@ -17,8 +17,8 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "path_utils.hpp" -#include -#include +#include +#include #include #include #include @@ -37,11 +37,8 @@ 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; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; RunOutModule::RunOutModule( const int64_t module_id, const std::shared_ptr & planner_data, diff --git a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp index 85da1340a5a5d..e3fd30a4e36ce 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/scene.hpp @@ -21,7 +21,7 @@ #include "state_machine.hpp" #include "utils.hpp" -#include +#include #include #include @@ -39,11 +39,6 @@ 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 { diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp index ec1cd460e1bb0..ed745cbb92679 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.cpp @@ -35,9 +35,6 @@ #include 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) diff --git a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp index 10f856b257a61..28bfa9569c66d 100644 --- a/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp +++ b/planning/autoware_behavior_velocity_run_out_module/src/utils.hpp @@ -17,8 +17,8 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include -#include +#include +#include #include #include @@ -38,8 +38,6 @@ 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; diff --git a/planning/autoware_behavior_velocity_template_module/package.xml b/planning/autoware_behavior_velocity_template_module/package.xml index a6bcbf5c34e76..eb6a6668fcee9 100644 --- a/planning/autoware_behavior_velocity_template_module/package.xml +++ b/planning/autoware_behavior_velocity_template_module/package.xml @@ -13,8 +13,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.cpp b/planning/autoware_behavior_velocity_template_module/src/manager.cpp index 8f6621aaab835..8aaf6be9bbfe9 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.cpp @@ -61,4 +61,4 @@ TemplateModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::TemplateModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_template_module/src/manager.hpp b/planning/autoware_behavior_velocity_template_module/src/manager.hpp index 8e95f516c337d..b92913b2a95ce 100644 --- a/planning/autoware_behavior_velocity_template_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,8 @@ namespace autoware::behavior_velocity_planner * * @param node A reference to the ROS node. */ -class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleManagerInterface +class TemplateModuleManager +: public autoware::behavior_velocity_planner::SceneModuleManagerInterface { public: explicit TemplateModuleManager(rclcpp::Node & node); @@ -85,7 +86,7 @@ class TemplateModuleManager : public ::behavior_velocity_planner::SceneModuleMan * Velocity Planner. */ class TemplateModulePlugin -: public ::behavior_velocity_planner::PluginWrapper +: public autoware::behavior_velocity_planner::PluginWrapper { }; diff --git a/planning/autoware_behavior_velocity_template_module/src/scene.hpp b/planning/autoware_behavior_velocity_template_module/src/scene.hpp index 3ce5ddbd8729d..0fc4e7dd9ae91 100644 --- a/planning/autoware_behavior_velocity_template_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_template_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include #include #include @@ -23,8 +23,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; using tier4_planning_msgs::msg::PathWithLaneId; class TemplateModule : public SceneModuleInterface diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml index cd35d4308c26a..cda3abbd0eee9 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml index 2402fc13469b9..2e2abef259778 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp index 5a32cfd2f74f0..39dca1f8f6303 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ namespace autoware::behavior_velocity_planner { using lanelet::autoware::VirtualTrafficLight; using tier4_autoware_utils::getOrDeclareParameter; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; +namespace planning_utils = autoware::behavior_velocity_planner::planning_utils; VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -82,4 +82,4 @@ VirtualTrafficLightModuleManager::getModuleExpiredFunction( #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::VirtualTrafficLightModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp index c73bb0d706008..6b0436a4290ac 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -29,9 +29,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; class VirtualTrafficLightModuleManager : public SceneModuleManagerInterface { public: diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp index 7d91a954171dd..a87d8867adf7e 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,11 +25,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PlanningBehavior; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::VelocityFactor; -namespace arc_lane_utils = ::behavior_velocity_planner::arc_lane_utils; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { using tier4_autoware_utils::calcDistance2d; @@ -181,7 +176,8 @@ std::optional insertStopVelocityAtCollision( auto insert_point = path->points.at(insert_index); insert_point.point.pose = interpolated_pose; // Insert 0 velocity after stop point or replace velocity with 0 - behavior_velocity_planner::planning_utils::insertVelocity(*path, insert_point, 0.0, insert_index); + autoware::behavior_velocity_planner::planning_utils::insertVelocity( + *path, insert_point, 0.0, insert_index); return insert_index; } } // namespace diff --git a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp index c83ff4e0607ef..9775e64145529 100644 --- a/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp +++ b/planning/autoware_behavior_velocity_virtual_traffic_light_module/src/scene.hpp @@ -15,7 +15,7 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include +#include #include #include #include @@ -32,10 +32,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::Pose; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; class VirtualTrafficLightModule : public SceneModuleInterface { public: diff --git a/planning/autoware_behavior_velocity_walkway_module/package.xml b/planning/autoware_behavior_velocity_walkway_module/package.xml index 59fa84acefcbb..bbe87ffb8b2cd 100644 --- a/planning/autoware_behavior_velocity_walkway_module/package.xml +++ b/planning/autoware_behavior_velocity_walkway_module/package.xml @@ -17,9 +17,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_planning_msgs behavior_velocity_crosswalk_module - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/autoware_behavior_velocity_walkway_module/plugins.xml b/planning/autoware_behavior_velocity_walkway_module/plugins.xml index d7f9e948154c1..9a5803807ae8d 100644 --- a/planning/autoware_behavior_velocity_walkway_module/plugins.xml +++ b/planning/autoware_behavior_velocity_walkway_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp index d26a47e512bc4..80badf59434da 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene_walkway.hpp" -#include +#include #include #include #include @@ -34,7 +34,6 @@ using tier4_autoware_utils::createMarkerColor; using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; namespace { diff --git a/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp index 1adea85788d5b..cf3efe3700a05 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -26,12 +26,8 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::SceneModuleManagerInterface; using lanelet::autoware::Crosswalk; using tier4_autoware_utils::getOrDeclareParameter; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; -using ::behavior_velocity_planner::getCrosswalkIdSetOnPath; -using ::behavior_velocity_planner::getCrosswalksOnPath; WalkwayModuleManager::WalkwayModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) @@ -109,4 +105,4 @@ WalkwayModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) #include PLUGINLIB_EXPORT_CLASS( autoware::behavior_velocity_planner::WalkwayModulePlugin, - ::behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp index d19bf9c1c1d5f..592181dd3ac75 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_walkway.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -33,9 +33,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::PluginWrapper; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::SceneModuleManagerInterface; using tier4_planning_msgs::msg::PathWithLaneId; class WalkwayModuleManager : public SceneModuleManagerInterface diff --git a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp index 236654e598216..73ae5a52e0183 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.cpp @@ -14,7 +14,7 @@ #include "scene_walkway.hpp" -#include +#include #include #include @@ -22,19 +22,11 @@ namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; -using ::behavior_velocity_planner::getLinestringIntersects; -using ::behavior_velocity_planner::getPolygonIntersects; -using ::behavior_velocity_planner::getStopLineFromMap; -using ::behavior_velocity_planner::PlanningBehavior; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopFactor; -using ::behavior_velocity_planner::VelocityFactor; using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findNearestSegmentIndex; using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPose; -namespace planning_utils = ::behavior_velocity_planner::planning_utils; WalkwayModule::WalkwayModule( const int64_t module_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, diff --git a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp index 1928466400134..e6c8f61ba656e 100644 --- a/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp +++ b/planning/autoware_behavior_velocity_walkway_module/src/scene_walkway.hpp @@ -18,7 +18,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" #include "scene_walkway.hpp" -#include +#include #include #include @@ -33,11 +33,6 @@ namespace autoware::behavior_velocity_planner { -using ::behavior_velocity_planner::DebugData; -using ::behavior_velocity_planner::PathWithLaneId; -using ::behavior_velocity_planner::SceneModuleInterface; -using ::behavior_velocity_planner::StopReason; - class WalkwayModule : public SceneModuleInterface { public: diff --git a/planning/costmap_generator/CMakeLists.txt b/planning/autoware_costmap_generator/CMakeLists.txt similarity index 80% rename from planning/costmap_generator/CMakeLists.txt rename to planning/autoware_costmap_generator/CMakeLists.txt index 620a997990001..b8b15a7312324 100644 --- a/planning/costmap_generator/CMakeLists.txt +++ b/planning/autoware_costmap_generator/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.14) -project(costmap_generator) +project(autoware_costmap_generator) find_package(autoware_cmake REQUIRED) autoware_package() @@ -16,9 +16,9 @@ include_directories( ) ament_auto_add_library(costmap_generator_lib SHARED - nodes/costmap_generator/points_to_costmap.cpp - nodes/costmap_generator/objects_to_costmap.cpp - nodes/costmap_generator/object_map_utils.cpp + nodes/autoware_costmap_generator/points_to_costmap.cpp + nodes/autoware_costmap_generator/objects_to_costmap.cpp + nodes/autoware_costmap_generator/object_map_utils.cpp ) target_link_libraries(costmap_generator_lib ${PCL_LIBRARIES} @@ -33,7 +33,7 @@ if(${PCL_VERSION} GREATER_EQUAL 1.12.1) endif() ament_auto_add_library(costmap_generator_node SHARED - nodes/costmap_generator/costmap_generator_node.cpp + nodes/autoware_costmap_generator/costmap_generator_node.cpp ) target_link_libraries(costmap_generator_node ${PCL_LIBRARIES} @@ -41,7 +41,7 @@ target_link_libraries(costmap_generator_node ) rclcpp_components_register_node(costmap_generator_node - PLUGIN "CostmapGenerator" + PLUGIN "autoware::costmap_generator::CostmapGenerator" EXECUTABLE costmap_generator ) diff --git a/planning/costmap_generator/README.md b/planning/autoware_costmap_generator/README.md similarity index 100% rename from planning/costmap_generator/README.md rename to planning/autoware_costmap_generator/README.md diff --git a/planning/costmap_generator/config/costmap_generator.param.yaml b/planning/autoware_costmap_generator/config/costmap_generator.param.yaml similarity index 100% rename from planning/costmap_generator/config/costmap_generator.param.yaml rename to planning/autoware_costmap_generator/config/costmap_generator.param.yaml diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp similarity index 94% rename from planning/costmap_generator/include/costmap_generator/costmap_generator.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp index 31c8a05cc3c60..093c88541e9c8 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/costmap_generator.hpp @@ -42,11 +42,11 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#define COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ -#include "costmap_generator/objects_to_costmap.hpp" -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include @@ -72,6 +72,8 @@ #include #include +namespace autoware::costmap_generator +{ class CostmapGenerator : public rclcpp::Node { public: @@ -197,5 +199,6 @@ class CostmapGenerator : public rclcpp::Node /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__COSTMAP_GENERATOR_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/object_map_utils.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp index 282bd6dcc1beb..f4911cc428d36 100644 --- a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/object_map_utils.hpp @@ -30,8 +30,8 @@ * */ -#ifndef COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ -#define COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ #include #include @@ -98,4 +98,4 @@ void FillPolygonAreas( } // namespace object_map -#endif // COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECT_MAP_UTILS_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp index a88bb97a623e2..aa11a98830dc3 100644 --- a/planning/costmap_generator/include/costmap_generator/objects_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/objects_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ #include @@ -57,6 +57,8 @@ #include +namespace autoware::costmap_generator +{ class ObjectsToCostmap { public: @@ -123,5 +125,5 @@ class ObjectsToCostmap const grid_map::Polygon & polygon, const std::string & gridmap_layer_name, const float score, grid_map::GridMap & objects_costmap); }; - -#endif // COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ +} // namespace autoware::costmap_generator +#endif // AUTOWARE_COSTMAP_GENERATOR__OBJECTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp similarity index 95% rename from planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp rename to planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp index 3b179d63f1d2a..0e3abbd69ec20 100644 --- a/planning/costmap_generator/include/costmap_generator/points_to_costmap.hpp +++ b/planning/autoware_costmap_generator/include/autoware_costmap_generator/points_to_costmap.hpp @@ -42,8 +42,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#ifndef COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ -#define COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#ifndef AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#define AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ #include @@ -52,6 +52,9 @@ #include #include +namespace autoware::costmap_generator + +{ class PointsToCostmap { public: @@ -115,5 +118,6 @@ class PointsToCostmap const std::string & gridmap_layer_name, const std::vector>> grid_vec); }; +} // namespace autoware::costmap_generator -#endif // COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ +#endif // AUTOWARE_COSTMAP_GENERATOR__POINTS_TO_COSTMAP_HPP_ diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml similarity index 84% rename from planning/costmap_generator/launch/costmap_generator.launch.xml rename to planning/autoware_costmap_generator/launch/costmap_generator.launch.xml index ba8a71f5f428a..e7c378565dd0f 100644 --- a/planning/costmap_generator/launch/costmap_generator.launch.xml +++ b/planning/autoware_costmap_generator/launch/costmap_generator.launch.xml @@ -6,9 +6,9 @@ - + - + diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp index 777dfc7a50a6e..4302ac6ada645 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/costmap_generator_node.cpp @@ -42,8 +42,8 @@ * OF private_node SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/costmap_generator.hpp" -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/costmap_generator.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include @@ -157,6 +157,8 @@ pcl::PointCloud getTransformedPointCloud( } // namespace +namespace autoware::costmap_generator +{ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) : Node("costmap_generator", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) { @@ -476,6 +478,7 @@ void CostmapGenerator::publishCostmap(const grid_map::GridMap & costmap) out_gridmap_msg->header = header; pub_costmap_->publish(*out_gridmap_msg); } +} // namespace autoware::costmap_generator #include -RCLCPP_COMPONENTS_REGISTER_NODE(CostmapGenerator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::costmap_generator::CostmapGenerator) diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp index 6955798be3e8a..4ce452814008c 100644 --- a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/object_map_utils.cpp @@ -30,7 +30,7 @@ * */ -#include "costmap_generator/object_map_utils.hpp" +#include "autoware_costmap_generator/object_map_utils.hpp" #include #include diff --git a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp index f6f024fb92a4e..afb2dbb9874c9 100644 --- a/planning/costmap_generator/nodes/costmap_generator/objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/objects_to_costmap.cpp @@ -42,13 +42,15 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/objects_to_costmap.hpp" +#include "autoware_costmap_generator/objects_to_costmap.hpp" #include #include #include +namespace autoware::costmap_generator +{ // Constructor ObjectsToCostmap::ObjectsToCostmap() : NUMBER_OF_POINTS(4), @@ -196,3 +198,4 @@ grid_map::Matrix ObjectsToCostmap::makeCostmapFromObjects( return objects_costmap[OBJECTS_COSTMAP_LAYER_]; } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp rename to planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp index e875f68973a12..723c27201ac8a 100644 --- a/planning/costmap_generator/nodes/costmap_generator/points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/nodes/autoware_costmap_generator/points_to_costmap.cpp @@ -42,11 +42,14 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ********************/ -#include "costmap_generator/points_to_costmap.hpp" +#include "autoware_costmap_generator/points_to_costmap.hpp" #include #include +namespace autoware::costmap_generator +{ + void PointsToCostmap::initGridmapParam(const grid_map::GridMap & gridmap) { grid_length_x_ = gridmap.getLength().x(); @@ -140,3 +143,5 @@ grid_map::Matrix PointsToCostmap::makeCostmapFromPoints( gridmap_layer_name, grid_vec); return costmap; } + +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/package.xml b/planning/autoware_costmap_generator/package.xml similarity index 92% rename from planning/costmap_generator/package.xml rename to planning/autoware_costmap_generator/package.xml index dc94d74dce8ae..aa1ae80eaf953 100644 --- a/planning/costmap_generator/package.xml +++ b/planning/autoware_costmap_generator/package.xml @@ -1,9 +1,9 @@ - costmap_generator + autoware_costmap_generator 0.1.0 - The costmap_generator package + The autoware_costmap_generator package Kosuke Takeuchi Takamasa Horibe Takayuki Murooka diff --git a/planning/costmap_generator/schema/costmap_generator.schema.json b/planning/autoware_costmap_generator/schema/costmap_generator.schema.json similarity index 100% rename from planning/costmap_generator/schema/costmap_generator.schema.json rename to planning/autoware_costmap_generator/schema/costmap_generator.schema.json diff --git a/planning/costmap_generator/test/test_objects_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp similarity index 97% rename from planning/costmap_generator/test/test_objects_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp index 15b853d0782aa..8d437865b8793 100644 --- a/planning/costmap_generator/test/test_objects_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_objects_to_costmap.cpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include #include +namespace autoware::costmap_generator +{ using LABEL = autoware_perception_msgs::msg::ObjectClassification; class ObjectsToCostMapTest : public ::testing::Test @@ -134,3 +136,4 @@ TEST_F(ObjectsToCostMapTest, TestMakeCostmapFromObjects) } } } +} // namespace autoware::costmap_generator diff --git a/planning/costmap_generator/test/test_points_to_costmap.cpp b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp similarity index 98% rename from planning/costmap_generator/test/test_points_to_costmap.cpp rename to planning/autoware_costmap_generator/test/test_points_to_costmap.cpp index 2366dffc7fc6f..6c52e2dc6ce9d 100644 --- a/planning/costmap_generator/test/test_points_to_costmap.cpp +++ b/planning/autoware_costmap_generator/test/test_points_to_costmap.cpp @@ -12,9 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include + +namespace autoware::costmap_generator +{ using pointcloud = pcl::PointCloud; class PointsToCostmapTest : public ::testing::Test { @@ -216,3 +219,4 @@ TEST_F(PointsToCostmapTest, TestMakeCostmapFromPoints_invalidPoints_outOfGrid) EXPECT_EQ(nonempty_grid_cell_num, 0); } +} // namespace autoware::costmap_generator diff --git a/planning/autoware_path_optimizer/CMakeLists.txt b/planning/autoware_path_optimizer/CMakeLists.txt index 3ceeae1022b10..78077e6c42720 100644 --- a/planning/autoware_path_optimizer/CMakeLists.txt +++ b/planning/autoware_path_optimizer/CMakeLists.txt @@ -41,7 +41,7 @@ target_include_directories(autoware_path_optimizer # register node rclcpp_components_register_node(autoware_path_optimizer - PLUGIN "autoware_path_optimizer::PathOptimizer" + PLUGIN "autoware::path_optimizer::PathOptimizer" EXECUTABLE path_optimizer_node ) diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp index b27eff787ca5a..d2de8d201d9fa 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/common_structs.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; struct Bounds; @@ -153,6 +153,6 @@ struct EgoNearestParam double dist_threshold{0.0}; double yaw_threshold{0.0}; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp index d1d2abaeaad61..edca8d706047b 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/debug_marker.hpp @@ -25,11 +25,11 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { MarkerArray getDebugMarker( const DebugData & debug_data, const std::vector & optimized_points, const vehicle_info_util::VehicleInfo & vehicle_info, const bool publish_extra_marker); -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__DEBUG_MARKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp index 33da339c62e40..8c207a9a3830f 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/mpt_optimizer.hpp @@ -34,7 +34,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct Bounds { @@ -308,5 +308,5 @@ class MPTOptimizer size_t getNumberOfSlackVariables() const; std::optional calcNormalizedAvoidanceCost(const ReferencePoint & ref_point) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp index a0e776628e3cb..30c95debe11cb 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/node.hpp @@ -33,7 +33,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class PathOptimizer : public rclcpp::Node { @@ -141,6 +141,6 @@ class PathOptimizer : public rclcpp::Node std::unique_ptr published_time_publisher_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__NODE_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp index 8270bb631e0ae..26d9599f07091 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/replan_checker.hpp @@ -24,7 +24,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { class ReplanChecker { @@ -66,6 +66,6 @@ class ReplanChecker bool isPathGoalChanged( const PlannerData & planner_data, const std::vector & prev_traj_points) const; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__REPLAN_CHECKER_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp index 6ee4fcb0ab7a5..0c41da0ee5f62 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/state_equation_generator.hpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { struct ReferencePoint; @@ -58,5 +58,5 @@ class StateEquationGenerator std::unique_ptr vehicle_model_ptr_; mutable std::shared_ptr time_keeper_ptr_; }; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__STATE_EQUATION_GENERATOR_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp index 755d0b2ace297..63a5840789a6b 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/type_alias.hpp @@ -28,7 +28,7 @@ #include "tier4_debug_msgs/msg/string_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // std_msgs using std_msgs::msg::Header; @@ -45,6 +45,6 @@ using visualization_msgs::msg::MarkerArray; // debug using tier4_debug_msgs::msg::Float64Stamped; using tier4_debug_msgs::msg::StringStamped; -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp index 32ef8cd5941fc..588b68f52a094 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/geometry_utils.hpp @@ -39,13 +39,13 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace geometry_utils { @@ -68,5 +68,5 @@ bool isOutsideDrivableAreaFromRectangleFootprint( const vehicle_info_util::VehicleInfo & vehicle_info, const bool use_footprint_polygon_for_outside_drivable_area_check); } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__GEOMETRY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp index 1056d80ef37aa..f3f528df2ec42 100644 --- a/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp +++ b/planning/autoware_path_optimizer/include/autoware_path_optimizer/utils/trajectory_utils.hpp @@ -38,16 +38,16 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p); template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p); +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p); template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p); +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p); } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -214,5 +214,5 @@ void insertStopPoint( std::vector & traj_points, const geometry_msgs::msg::Pose & input_stop_pose, const size_t stop_seg_idx); } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #endif // AUTOWARE_PATH_OPTIMIZER__UTILS__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_path_optimizer/src/debug_marker.cpp b/planning/autoware_path_optimizer/src/debug_marker.cpp index 3127d521160c2..7c644f4448a0c 100644 --- a/planning/autoware_path_optimizer/src/debug_marker.cpp +++ b/planning/autoware_path_optimizer/src/debug_marker.cpp @@ -18,7 +18,7 @@ #include "visualization_msgs/msg/marker_array.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -432,4 +432,4 @@ MarkerArray getDebugMarker( return marker_array; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp index ece301e64c97e..2ab622c6b4b58 100644 --- a/planning/autoware_path_optimizer/src/mpt_optimizer.cpp +++ b/planning/autoware_path_optimizer/src/mpt_optimizer.cpp @@ -29,7 +29,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -1783,4 +1783,4 @@ std::optional MPTOptimizer::calcNormalizedAvoidanceCost( } return std::clamp(-negative_avoidance_cost / mpt_param_.max_avoidance_cost, 0.0, 1.0); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/node.cpp b/planning/autoware_path_optimizer/src/node.cpp index 49d41e6b07884..816c0d459d95f 100644 --- a/planning/autoware_path_optimizer/src/node.cpp +++ b/planning/autoware_path_optimizer/src/node.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace { @@ -666,7 +666,7 @@ void PathOptimizer::publishDebugData(const Header & header) const time_keeper_ptr_->toc(__func__, " "); } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_path_optimizer::PathOptimizer) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::path_optimizer::PathOptimizer) diff --git a/planning/autoware_path_optimizer/src/replan_checker.cpp b/planning/autoware_path_optimizer/src/replan_checker.cpp index 797041ee75416..6745872f50b02 100644 --- a/planning/autoware_path_optimizer/src/replan_checker.cpp +++ b/planning/autoware_path_optimizer/src/replan_checker.cpp @@ -21,7 +21,7 @@ #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { ReplanChecker::ReplanChecker(rclcpp::Node * node, const EgoNearestParam & ego_nearest_param) : ego_nearest_param_(ego_nearest_param), logger_(node->get_logger().get_child("replan_checker")) @@ -212,4 +212,4 @@ bool ReplanChecker::isPathGoalChanged( return true; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/state_equation_generator.cpp b/planning/autoware_path_optimizer/src/state_equation_generator.cpp index 7712fbbf6c3cf..ec6a9e575be95 100644 --- a/planning/autoware_path_optimizer/src/state_equation_generator.cpp +++ b/planning/autoware_path_optimizer/src/state_equation_generator.cpp @@ -16,7 +16,7 @@ #include "autoware_path_optimizer/mpt_optimizer.hpp" -namespace autoware_path_optimizer +namespace autoware::path_optimizer { // state equation: x = B u + W (u includes x_0) // NOTE: Originally, x_t+1 = Ad x_t + Bd u + Wd. @@ -69,4 +69,4 @@ Eigen::VectorXd StateEquationGenerator::predict( { return mat.B * U + mat.W; } -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp index 9d93cdc26a7ed..45302c0b729a9 100644 --- a/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/geometry_utils.cpp @@ -36,7 +36,7 @@ #include #include -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace bg = boost::geometry; using tier4_autoware_utils::LinearRing2d; @@ -207,4 +207,4 @@ bool isOutsideDrivableAreaFromRectangleFootprint( return false; } } // namespace geometry_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp index 7983c5c2a3c2f..75e2b75b232e0 100644 --- a/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp +++ b/planning/autoware_path_optimizer/src/utils/trajectory_utils.cpp @@ -36,25 +36,25 @@ namespace tier4_autoware_utils { template <> -geometry_msgs::msg::Point getPoint(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Point getPoint(const autoware::path_optimizer::ReferencePoint & p) { return p.pose.position; } template <> -geometry_msgs::msg::Pose getPose(const autoware_path_optimizer::ReferencePoint & p) +geometry_msgs::msg::Pose getPose(const autoware::path_optimizer::ReferencePoint & p) { return p.pose; } template <> -double getLongitudinalVelocity(const autoware_path_optimizer::ReferencePoint & p) +double getLongitudinalVelocity(const autoware::path_optimizer::ReferencePoint & p) { return p.longitudinal_velocity_mps; } } // namespace tier4_autoware_utils -namespace autoware_path_optimizer +namespace autoware::path_optimizer { namespace trajectory_utils { @@ -242,4 +242,4 @@ void insertStopPoint( traj_points.insert(traj_points.begin() + stop_seg_idx + 1, additional_traj_point); } } // namespace trajectory_utils -} // namespace autoware_path_optimizer +} // namespace autoware::path_optimizer diff --git a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp index 8ef099ba09f24..4af88539597be 100644 --- a/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp +++ b/planning/autoware_path_optimizer/test/test_path_optimizer_node_interface.cpp @@ -42,7 +42,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) planning_test_utils_dir + "/config/test_nearest_search.param.yaml", "--params-file", path_optimizer_dir + "/config/path_optimizer.param.yaml"}); - auto test_target_node = std::make_shared(node_options); + auto test_target_node = std::make_shared(node_options); // publish necessary topics from test_manager test_manager->publishOdometry(test_target_node, "path_optimizer/input/odometry"); diff --git a/planning/autoware_planning_test_manager/README.md b/planning/autoware_planning_test_manager/README.md index fe5484ca498ee..97eb2fd8d94fd 100644 --- a/planning/autoware_planning_test_manager/README.md +++ b/planning/autoware_planning_test_manager/README.md @@ -41,7 +41,7 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", - planning_validator_dir + "/config/planning_validator.param.yaml"}); + autoware_planning_validator_dir + "/config/planning_validator.param.yaml"}); // instantiate the TargetNode with node_options auto test_target_node = std::make_shared(node_options); @@ -70,18 +70,18 @@ TEST(PlanningModuleInterfaceTest, NodeTestWithExceptionTrajectory) ## Implemented tests -| Node | Test name | exceptional input | output | Exceptional input pattern | -| ------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | -| planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | -| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | -| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | -| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | -| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | +| Node | Test name | exceptional input | output | Exceptional input pattern | +| --------------------------- | ----------------------------------------------------------------------------------------- | ----------------- | -------------- | ------------------------------------------------------------------------------------- | +| autoware_planning_validator | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| velocity_smoother | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_cruise_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_stop_planner | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| obstacle_velocity_limiter | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| path_optimizer | NodeTestWithExceptionTrajectory | trajectory | trajectory | Empty, single point, path with duplicate points | +| scenario_selector | NodeTestWithExceptionTrajectoryLaneDrivingMode NodeTestWithExceptionTrajectoryParkingMode | trajectory | scenario | Empty, single point, path with duplicate points for scenarios:LANEDRIVING and PARKING | +| freespace_planner | NodeTestWithExceptionRoute | route | trajectory | Empty route | +| behavior_path_planner | NodeTestWithExceptionRoute NodeTestWithOffTrackEgoPose | route | route odometry | Empty route Off-lane ego-position | +| behavior_velocity_planner | NodeTestWithExceptionPathWithLaneID | path_with_lane_id | path | Empty path | ## Important Notes diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 9eb9a2b432a21..0705f0b51ddd3 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -130,7 +130,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra const auto eb_path_smoother_ptr = path_smoother::ElasticBandSmoother(create_node_options()).getElasticBandSmoother(); const auto mpt_optimizer_ptr = - autoware_path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); + autoware::path_optimizer::PathOptimizer(create_node_options()).getMPTOptimizer(); // NOTE: The optimization is executed every valid_optimized_traj_points_num points. constexpr int valid_optimized_traj_points_num = 10; @@ -158,7 +158,7 @@ std::vector OptimizationTrajectoryBasedCenterline::optimize_tra // road collision avoidance by model predictive trajectory in the autoware_path_optimizer // package - const autoware_path_optimizer::PlannerData planner_data{ + const autoware::path_optimizer::PlannerData planner_data{ raw_path.header, smoothed_traj_points, raw_path.left_bound, raw_path.right_bound, virtual_ego_pose}; const auto optimized_traj_points = mpt_optimizer_ptr->optimizeTrajectory(planner_data); diff --git a/planning/autoware_velocity_smoother/CMakeLists.txt b/planning/autoware_velocity_smoother/CMakeLists.txt index fee13513a7a97..9dd0c62c4537e 100644 --- a/planning/autoware_velocity_smoother/CMakeLists.txt +++ b/planning/autoware_velocity_smoother/CMakeLists.txt @@ -41,7 +41,7 @@ target_link_libraries(${PROJECT_NAME}_node ) rclcpp_components_register_node(${PROJECT_NAME}_node - PLUGIN "autoware_velocity_smoother::VelocitySmootherNode" + PLUGIN "autoware::velocity_smoother::VelocitySmootherNode" EXECUTABLE velocity_smoother_node ) diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp index aecfab342e7e0..8ab7acd5ec9b3 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/node.hpp @@ -53,7 +53,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -275,6 +275,6 @@ class VelocitySmootherNode : public rclcpp::Node std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__NODE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp index dc85c2b2f336f..24a20ca8a588f 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/resample.hpp @@ -20,7 +20,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -48,6 +48,6 @@ TrajectoryPoints resampleTrajectory( const double nearest_dist_threshold, const double nearest_yaw_threshold, const ResampleParam & param, const double nominal_ds, const bool use_zoh_for_v = true); } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__RESAMPLE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp index e54a6e7b8afad..a5baa59f5d034 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.hpp @@ -28,7 +28,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class AnalyticalJerkConstrainedSmoother : public SmootherBase { @@ -113,7 +113,7 @@ class AnalyticalJerkConstrainedSmoother : public SmootherBase std::string strTimes(const std::vector & times) const; std::string strStartIndices(const std::vector & start_indices) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp index 3ab4fce11fd0b..0c18e102f6f66 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.hpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -51,7 +51,7 @@ double integ_x(double x0, double v0, double a0, double j0, double t); double integ_v(double v0, double a0, double j0, double t); double integ_a(double a0, double j0, double t); } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother // clang-format off #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__ANALYTICAL_JERK_CONSTRAINED_SMOOTHER__VELOCITY_PLANNING_UTILS_HPP_ // NOLINT diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp index 09c1e7e96be7b..4e93fcd339140 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/jerk_filtered_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class JerkFilteredSmoother : public SmootherBase { @@ -69,6 +69,6 @@ class JerkFilteredSmoother : public SmootherBase const double v0, const double a0, const double a_min, const double j_min, const TrajectoryPoints & forward_filtered, const TrajectoryPoints & backward_filtered) const; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__JERK_FILTERED_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp index 066e0acef67f6..44fa263e71fb9 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/l2_pseudo_jerk_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class L2PseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class L2PseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("l2_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__L2_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp index b54de318e9702..f638e03658e05 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/linf_pseudo_jerk_smoother.hpp @@ -26,7 +26,7 @@ #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { class LinfPseudoJerkSmoother : public SmootherBase { @@ -56,6 +56,6 @@ class LinfPseudoJerkSmoother : public SmootherBase autoware::common::osqp::OSQPInterface qp_solver_; rclcpp::Logger logger_{rclcpp::get_logger("smoother").get_child("linf_pseudo_jerk_smoother")}; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__LINF_PSEUDO_JERK_SMOOTHER_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp index 43ccfcf193f14..0c17ef5bdbb9d 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/smoother/smoother_base.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -86,6 +86,6 @@ class SmootherBase protected: BaseParam base_param_; }; -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #endif // AUTOWARE_VELOCITY_SMOOTHER__SMOOTHER__SMOOTHER_BASE_HPP_ diff --git a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp index d35f80fb7fad8..f7999d1c8b04c 100644 --- a/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp +++ b/planning/autoware_velocity_smoother/include/autoware_velocity_smoother/trajectory_utils.hpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother::trajectory_utils +namespace autoware::velocity_smoother::trajectory_utils { using autoware_planning_msgs::msg::TrajectoryPoint; using TrajectoryPoints = std::vector; @@ -77,6 +77,6 @@ std::vector calcVelocityProfileWithConstantJerkAndAccelerationLimit( double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closest); -} // namespace autoware_velocity_smoother::trajectory_utils +} // namespace autoware::velocity_smoother::trajectory_utils #endif // AUTOWARE_VELOCITY_SMOOTHER__TRAJECTORY_UTILS_HPP_ diff --git a/planning/autoware_velocity_smoother/src/node.cpp b/planning/autoware_velocity_smoother/src/node.cpp index 0ea10534eb126..0fd83871153f0 100644 --- a/planning/autoware_velocity_smoother/src/node.cpp +++ b/planning/autoware_velocity_smoother/src/node.cpp @@ -32,7 +32,7 @@ #include // clang-format on -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { VelocitySmootherNode::VelocitySmootherNode(const rclcpp::NodeOptions & node_options) : Node("velocity_smoother", node_options) @@ -1099,7 +1099,7 @@ TrajectoryPoint VelocitySmootherNode::calcProjectedTrajectoryPointFromEgo( return calcProjectedTrajectoryPoint(trajectory, current_odometry_ptr_->pose.pose); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(autoware_velocity_smoother::VelocitySmootherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::velocity_smoother::VelocitySmootherNode) diff --git a/planning/autoware_velocity_smoother/src/resample.cpp b/planning/autoware_velocity_smoother/src/resample.cpp index 17ea1eb9fc9bd..af3b222a71ef9 100644 --- a/planning/autoware_velocity_smoother/src/resample.cpp +++ b/planning/autoware_velocity_smoother/src/resample.cpp @@ -22,7 +22,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace resampling { @@ -264,4 +264,4 @@ TrajectoryPoints resampleTrajectory( } } // namespace resampling -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp index 9cdfc515d51b7..dc5f4f19ce771 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/analytical_jerk_constrained_smoother.cpp @@ -64,7 +64,7 @@ bool applyMaxVelocity( } // namespace -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { AnalyticalJerkConstrainedSmoother::AnalyticalJerkConstrainedSmoother(rclcpp::Node & node) : SmootherBase(node) @@ -658,4 +658,4 @@ std::string AnalyticalJerkConstrainedSmoother::strStartIndices( return ss.str(); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp index 84ac29d14bdc5..e3edcfbbec681 100644 --- a/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/analytical_jerk_constrained_smoother/velocity_planning_utils.cpp @@ -19,7 +19,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace analytical_velocity_planning_utils { @@ -353,4 +353,4 @@ double integ_a(double a0, double j0, double t) } } // namespace analytical_velocity_planning_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp index d212cfc0bcedf..d759930eb4320 100644 --- a/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/jerk_filtered_smoother.cpp @@ -27,7 +27,7 @@ #define VERBOSE_TRAJECTORY_VELOCITY false -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { JerkFilteredSmoother::JerkFilteredSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -480,4 +480,4 @@ TrajectoryPoints JerkFilteredSmoother::resampleTrajectory( smoother_param_.jerk_filter_ds); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp index 3c2872f2e58e3..fff1166567aee 100644 --- a/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/l2_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { L2PseudoJerkSmoother::L2PseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -242,4 +242,4 @@ TrajectoryPoints L2PseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp index f434d8d4514ca..b00936f3092ad 100644 --- a/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/linf_pseudo_jerk_smoother.cpp @@ -23,7 +23,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { LinfPseudoJerkSmoother::LinfPseudoJerkSmoother(rclcpp::Node & node) : SmootherBase(node) { @@ -254,4 +254,4 @@ TrajectoryPoints LinfPseudoJerkSmoother::resampleTrajectory( base_param_.resample_param); } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp index 5704113067244..e1d72889203cc 100644 --- a/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp +++ b/planning/autoware_velocity_smoother/src/smoother/smoother_base.cpp @@ -26,7 +26,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { namespace @@ -270,4 +270,4 @@ TrajectoryPoints SmootherBase::applySteeringRateLimit( return output; } -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp index aff1a0b0e3213..6ebf246748666 100644 --- a/planning/autoware_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/autoware_velocity_smoother/src/trajectory_utils.cpp @@ -25,7 +25,7 @@ #include #include -namespace autoware_velocity_smoother +namespace autoware::velocity_smoother { using geometry_msgs::msg::Point; namespace trajectory_utils @@ -610,4 +610,4 @@ double calcStopDistance(const TrajectoryPoints & trajectory, const size_t closes } } // namespace trajectory_utils -} // namespace autoware_velocity_smoother +} // namespace autoware::velocity_smoother diff --git a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp index 51ee84f535ca4..f1b7dbb197ea2 100644 --- a/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp +++ b/planning/autoware_velocity_smoother/test/test_smoother_functions.cpp @@ -18,8 +18,8 @@ #include +using autoware::velocity_smoother::trajectory_utils::TrajectoryPoints; using autoware_planning_msgs::msg::TrajectoryPoint; -using autoware_velocity_smoother::trajectory_utils::TrajectoryPoints; TrajectoryPoints genStraightTrajectory(const size_t size) { @@ -48,7 +48,7 @@ TEST(TestTrajectoryUtils, CalcTrajectoryCurvatureFrom3Points) const auto checkOutputSize = [](const size_t trajectory_size, const size_t idx_dist) { const auto trajectory_points = genStraightTrajectory(trajectory_size); const auto curvatures = - autoware_velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( + autoware::velocity_smoother::trajectory_utils::calcTrajectoryCurvatureFrom3Points( trajectory_points, idx_dist); EXPECT_EQ(curvatures.size(), trajectory_size) << ", idx_dist = " << idx_dist; }; diff --git a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp index f0145ea5a32a7..30a64955366e4 100644 --- a/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp +++ b/planning/autoware_velocity_smoother/test/test_velocity_smoother_node_interface.cpp @@ -22,7 +22,7 @@ #include -using autoware_velocity_smoother::VelocitySmootherNode; +using autoware::velocity_smoother::VelocitySmootherNode; using planning_test_utils::PlanningInterfaceTestManager; std::shared_ptr generateTestManager() diff --git a/planning/behavior_velocity_blind_spot_module/package.xml b/planning/behavior_velocity_blind_spot_module/package.xml index ebf760405a256..67947398afb01 100644 --- a/planning/behavior_velocity_blind_spot_module/package.xml +++ b/planning/behavior_velocity_blind_spot_module/package.xml @@ -16,9 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension motion_utils diff --git a/planning/behavior_velocity_blind_spot_module/plugins.xml b/planning/behavior_velocity_blind_spot_module/plugins.xml index 7dda59ea2fdbe..fb5019de5df08 100644 --- a/planning/behavior_velocity_blind_spot_module/plugins.xml +++ b/planning/behavior_velocity_blind_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index a30836ab9f6a9..c357df481bca8 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include @@ -25,7 +25,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -107,4 +107,4 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp index 6e4135c50b253..6b963d86c9bbf 100644 --- a/planning/behavior_velocity_blind_spot_module/src/decisions.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/decisions.cpp @@ -16,7 +16,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /* @@ -150,4 +150,4 @@ void BlindSpotModule::reactRTCApprovalByDecision( return; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index f1b0a56018d9b..7f2f7ebd89baa 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -14,8 +14,8 @@ #include "manager.hpp" -#include -#include +#include +#include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) @@ -95,8 +95,9 @@ BlindSpotModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::BlindSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::BlindSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.hpp b/planning/behavior_velocity_blind_spot_module/src/manager.hpp index 7d6f936cc9d5b..12dccd846affc 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class BlindSpotModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class BlindSpotModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 14ebe70216d5a..eed767fb891f9 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -14,9 +14,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include #include @@ -40,7 +40,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -704,4 +704,4 @@ bool BlindSpotModule::isTargetObjectType( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index b3bf90f5b91d1..cfb6bfdfeeded 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -15,9 +15,9 @@ #ifndef SCENE_HPP_ #define SCENE_HPP_ -#include -#include -#include +#include +#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /** * @brief wrapper class of interpolated path with lane id @@ -237,6 +237,6 @@ class BlindSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 690eca196d536..ca07636c2f60e 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -33,12 +33,12 @@ #define EIGEN_MPL2_ONLY #include #include -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathPointWithLaneId; @@ -108,6 +108,6 @@ std::vector getLinestringIntersects( std::optional getStopLineFromMap( const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // BEHAVIOR_VELOCITY_CROSSWALK_MODULE__UTIL_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2fd5e2bb514c3..eebf9c4d4a14f 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -21,9 +21,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs grid_map_core diff --git a/planning/behavior_velocity_crosswalk_module/plugins.xml b/planning/behavior_velocity_crosswalk_module/plugins.xml index 3d1d720857c17..e38ab0d85a63a 100644 --- a/planning/behavior_velocity_crosswalk_module/plugins.xml +++ b/planning/behavior_velocity_crosswalk_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_crosswalk_module/src/debug.cpp b/planning/behavior_velocity_crosswalk_module/src/debug.cpp index 4bee98db2fa56..1418d46081fcd 100644 --- a/planning/behavior_velocity_crosswalk_module/src/debug.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene_crosswalk.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; @@ -207,4 +207,4 @@ visualization_msgs::msg::MarkerArray CrosswalkModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index fe4282893674c..428ec2510c7f4 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::Crosswalk; @@ -229,8 +229,9 @@ CrosswalkModuleManager::getModuleExpiredFunction(const PathWithLaneId & path) return crosswalk_id_set.count(scene_module->getModuleId()) == 0; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::CrosswalkModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::CrosswalkModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.hpp b/planning/behavior_velocity_crosswalk_module/src/manager.hpp index ba7e75193deab..520dab49b347f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_crosswalk.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -56,6 +56,6 @@ class CrosswalkModuleManager : public SceneModuleManagerInterfaceWithRTC class CrosswalkModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp index b6adb32f642eb..619e389f3fcf8 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp @@ -22,11 +22,11 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::Index idx_offset; for (idx_offset.x() = 0; idx_offset.x() < min_nb_of_cells; ++idx_offset.x()) { @@ -140,7 +140,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params) + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params) { grid_map::GridMap grid_map; grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); @@ -170,4 +170,4 @@ double calculate_detection_range( const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity); return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp index 4aab9d3bfc888..c4f2c1a23c57e 100644 --- a/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { /// @brief check if the gridmap is occluded at the given index /// @param [in] grid_map input grid map @@ -40,7 +40,7 @@ namespace behavior_velocity_planner /// @return true if the index is occluded bool is_occluded( const grid_map::GridMap & grid_map, const int min_nb_of_cells, const grid_map::Index idx, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief interpolate a point beyond the end of the given segment /// @param [in] segment input segment @@ -62,7 +62,7 @@ bool is_crosswalk_occluded( const nav_msgs::msg::OccupancyGrid & occupancy_grid, const geometry_msgs::msg::Point & path_intersection, const double detection_range, const std::vector & dynamic_objects, - const behavior_velocity_planner::CrosswalkModule::PlannerParam & params); + const autoware::behavior_velocity_planner::CrosswalkModule::PlannerParam & params); /// @brief calculate the distance away from the crosswalk that should be checked for occlusions /// @param occluded_objects_velocity assumed velocity of the objects coming out of occlusions @@ -89,6 +89,6 @@ std::vector select_and_inflate_o void clear_occlusions_behind_objects( grid_map::GridMap & grid_map, const std::vector & objects); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUDED_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 191eea8feabed..91893c5224550 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,8 +16,8 @@ #include "occluded_crosswalk.hpp" -#include -#include +#include +#include #include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcArcLength; @@ -1281,4 +1281,4 @@ void CrosswalkModule::planStop( ego_path.points, planner_data_->current_odometry->pose, stop_factor->stop_pose, VelocityFactor::UNKNOWN); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 107a904dd076b..13ecddafb6490 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -17,7 +17,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using autoware_perception_msgs::msg::ObjectClassification; @@ -459,6 +459,6 @@ class CrosswalkModule : public SceneModuleInterface std::optional current_initial_occlusion_time_; std::optional most_recent_occlusion_time_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_CROSSWALK_HPP_ diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index d276ae95e06b6..883b40b89730f 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -14,7 +14,7 @@ #include "behavior_velocity_crosswalk_module/util.hpp" -#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcSignedArcLength; @@ -62,16 +62,19 @@ std::vector> getCrosswalksOnPath( // Add current lane id const auto nearest_lane_id = - behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); + autoware::behavior_velocity_planner::planning_utils::getNearestLaneId( + path, lanelet_map, current_pose); std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id - unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( - path, *nearest_lane_id); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( + path, *nearest_lane_id); } else { // Add all lane_ids in path - unique_lane_ids = behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); + unique_lane_ids = + autoware::behavior_velocity_planner::planning_utils::getSortedLaneIdsFromPath(path); } for (const auto lane_id : unique_lane_ids) { @@ -229,4 +232,4 @@ std::optional getStopLineFromMap( return stop_line.front(); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/package.xml b/planning/behavior_velocity_detection_area_module/package.xml index 6bcca43edc0cb..b2d370380829a 100644 --- a/planning/behavior_velocity_detection_area_module/package.xml +++ b/planning/behavior_velocity_detection_area_module/package.xml @@ -17,8 +17,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_detection_area_module/plugins.xml b/planning/behavior_velocity_detection_area_module/plugins.xml index 73497c8bfdf2a..9356c863ec26e 100644 --- a/planning/behavior_velocity_detection_area_module/plugins.xml +++ b/planning/behavior_velocity_detection_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_detection_area_module/src/debug.cpp b/planning/behavior_velocity_detection_area_module/src/debug.cpp index ee2af54e5ea2a..1ce982cd8df52 100644 --- a/planning/behavior_velocity_detection_area_module/src/debug.cpp +++ b/planning/behavior_velocity_detection_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include @@ -29,7 +29,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using std_msgs::msg::ColorRGBA; using tier4_autoware_utils::appendMarkerArray; @@ -191,4 +191,4 @@ motion_utils::VirtualWalls DetectionAreaModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/manager.cpp b/planning/behavior_velocity_detection_area_module/src/manager.cpp index feb5bf6bb50ef..336a1710ed593 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.cpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::DetectionArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -84,8 +84,9 @@ DetectionAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DetectionAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DetectionAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_detection_area_module/src/manager.hpp b/planning/behavior_velocity_detection_area_module/src/manager.hpp index 71cfa0a5eef96..64a76171814b9 100644 --- a/planning/behavior_velocity_detection_area_module/src/manager.hpp +++ b/planning/behavior_velocity_detection_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DetectionAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class DetectionAreaModulePlugin : public PluginWrapper -#include +#include +#include #include #ifdef ROS_DISTRO_GALACTIC #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; using motion_utils::calcLongitudinalOffsetPose; @@ -339,4 +339,4 @@ bool DetectionAreaModule::hasEnoughBrakingDistance( return arc_lane_utils::calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_detection_area_module/src/scene.hpp b/planning/behavior_velocity_detection_area_module/src/scene.hpp index f135c3b2660cb..f577cd92dea6e 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.hpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -101,6 +101,6 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml index 84d01d04a09a4..6b9db657d6d8e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/package.xml @@ -12,9 +12,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs libboost-dev motion_utils diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml index 2f5662c7998ac..ba2d2ba61ad50 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp index 8497369917232..10843fd3aef2b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { std::optional find_closest_collision_point( @@ -80,4 +80,4 @@ std::vector find_collisions( return collisions; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp index 27a48afa032b1..8431221a27d6b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/collision.hpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief find the collision point closest to ego along an object footprint /// @param [in] ego_data ego data including its path and footprints used for finding a collision @@ -41,6 +41,6 @@ std::vector find_collisions( const std::vector & objects, const tier4_autoware_utils::MultiPolygon2d & obstacle_forward_footprints); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // COLLISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp index 367a1c2be1d5a..5a457d651cd9e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.cpp @@ -23,7 +23,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( @@ -110,4 +110,4 @@ std::vector make_polygon_markers( } return markers; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp index 7397f63ca079c..c567948927089 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/debug.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug { std::vector make_delete_markers( const size_t from, const size_t to, const std::string & ns); @@ -40,6 +40,6 @@ std::vector make_collision_markers( const rclcpp::Time & now); std::vector make_polygon_markers( const tier4_autoware_utils::MultiPolygon2d & footprints, const std::string & ns, const double z); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop::debug +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp index a58e4e9b88c75..5f005348c4cea 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.cpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { tier4_autoware_utils::MultiPolygon2d make_forward_footprints( const std::vector & obstacles, @@ -81,4 +81,4 @@ void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params) } } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp index 0050a4e2c9259..c75d7bb9cc49e 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief create the footprint of the given obstacles and their projection over a fixed time /// horizon @@ -50,6 +50,6 @@ tier4_autoware_utils::Polygon2d project_to_pose( /// @param [inout] ego_data ego data with its path and the rtree to populate /// @param [in] params parameters void make_ego_footprint_rtree(EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp index 63f1f3025f94a..ddb8c842fb92d 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -68,9 +68,9 @@ DynamicObstacleStopModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::DynamicObstacleStopModulePlugin, - behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::DynamicObstacleStopModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp index eb7bf4c6dbc98..d5c403e26792c 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_dynamic_obstacle_stop.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class DynamicObstacleStopModuleManager : public SceneModuleManagerInterface { @@ -53,6 +53,6 @@ class DynamicObstacleStopModulePlugin : public PluginWrapper #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -91,4 +91,4 @@ std::vector filter_predicted_obj } return filtered_objects; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp index 5daa0cda44203..71da351d2f10b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief filter the given predicted objects @@ -34,6 +34,6 @@ std::vector filter_predicted_obj const autoware_perception_msgs::msg::PredictedObjects & objects, const EgoData & ego_data, const PlannerParam & params, const double hysteresis); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_FILTERING_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp index 1df8a1ed6a4a3..0f6fdbfadbf86 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.cpp @@ -18,7 +18,7 @@ #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { void update_object_map( ObjectStopDecisionMap & object_map, const std::vector & collisions, @@ -66,4 +66,4 @@ std::optional find_earliest_collision( return earliest_collision; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp index 5756c8589fb73..cc13614e80782 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_stop_decision.hpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { /// @brief representation of a decision to stop for a dynamic object struct ObjectStopDecision @@ -71,6 +71,6 @@ void update_object_map( std::optional find_earliest_collision( const ObjectStopDecisionMap & object_map, const EgoData & ego_data); -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // OBJECT_STOP_DECISION_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp index 11ec540e3980f..9c5eced187aa8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp @@ -21,8 +21,8 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using visualization_msgs::msg::Marker; @@ -163,4 +163,4 @@ motion_utils::VirtualWalls DynamicObstacleStopModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp index 79911c40a195d..8018412ba2b4b 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.hpp @@ -18,7 +18,7 @@ #include "object_stop_decision.hpp" #include "types.hpp" -#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { class DynamicObstacleStopModule : public SceneModuleInterface { @@ -57,6 +57,6 @@ class DynamicObstacleStopModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // SCENE_DYNAMIC_OBSTACLE_STOP_HPP_ diff --git a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp index 532d770608dd3..32ef89413b2f8 100644 --- a/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp +++ b/planning/behavior_velocity_dynamic_obstacle_stop_module/src/types.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner::dynamic_obstacle_stop +namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop { using BoxIndexPair = std::pair; using Rtree = boost::geometry::index::rtree>; @@ -84,6 +84,6 @@ struct Collision geometry_msgs::msg::Point point; std::string object_uuid; }; -} // namespace behavior_velocity_planner::dynamic_obstacle_stop +} // namespace autoware::behavior_velocity_planner::dynamic_obstacle_stop #endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/package.xml b/planning/behavior_velocity_no_drivable_lane_module/package.xml index 79eea68142cc8..4c3d6dbf78153 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/package.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/package.xml @@ -19,8 +19,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml index 33b0d5d6e7469..e3e9efb254fba 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/plugins.xml +++ b/planning/behavior_velocity_no_drivable_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp index 0c6bb747a854b..0908fe712dbf5 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/debug.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createDefaultMarker; @@ -102,4 +102,4 @@ visualization_msgs::msg::MarkerArray NoDrivableLaneModule::createDebugMarkerArra return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp index 0b7a8a8c28dd5..8c3fc5ef0f3ce 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.cpp @@ -14,12 +14,12 @@ #include "manager.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -66,8 +66,9 @@ NoDrivableLaneModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoDrivableLaneModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoDrivableLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp index 90455bd4b1c62..4afa498a5679b 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoDrivableLaneModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class NoDrivableLaneModulePlugin : public PluginWrapper +#include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::createPoint; @@ -283,4 +283,4 @@ void NoDrivableLaneModule::initialize_debug_data( } } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp index fb90e023fedcd..6910e972a8685 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -85,6 +85,6 @@ class NoDrivableLaneModule : public SceneModuleInterface void initialize_debug_data( const lanelet::Lanelet & no_drivable_lane, const geometry_msgs::msg::Point & ego_pos); }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp index 4c5efad94dfc8..c9a6ce1ac1510 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.cpp @@ -17,14 +17,14 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -110,4 +110,4 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP } return path_no_drivable_lane_polygon_intersection; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp index 11953fd5dd177..289b04a2ce96d 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/util.hpp @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -51,6 +51,6 @@ PathWithNoDrivableLanePolygonIntersection getPathIntersectionWithNoDrivableLaneP const PathWithLaneId & ego_path, const lanelet::BasicPolygon2d & polygon, const geometry_msgs::msg::Point & ego_pos, const size_t max_num); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_no_stopping_area_module/package.xml b/planning/behavior_velocity_no_stopping_area_module/package.xml index 0991b37120a6f..baa51d6681490 100644 --- a/planning/behavior_velocity_no_stopping_area_module/package.xml +++ b/planning/behavior_velocity_no_stopping_area_module/package.xml @@ -17,9 +17,9 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs interpolation diff --git a/planning/behavior_velocity_no_stopping_area_module/plugins.xml b/planning/behavior_velocity_no_stopping_area_module/plugins.xml index a9b07297cfa30..23d2a800c3cf5 100644 --- a/planning/behavior_velocity_no_stopping_area_module/plugins.xml +++ b/planning/behavior_velocity_no_stopping_area_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp index 9e51afca2d475..e90273d4ac5e4 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/debug.cpp @@ -14,8 +14,8 @@ #include "scene_no_stopping_area.hpp" -#include -#include +#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -175,4 +175,4 @@ motion_utils::VirtualWalls NoStoppingAreaModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp index faa265afe1559..6aeac1e9d8dd3 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::NoStoppingArea; using tier4_autoware_utils::getOrDeclareParameter; @@ -86,8 +86,9 @@ NoStoppingAreaModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::NoStoppingAreaModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::NoStoppingAreaModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp index baf5901ccc124..696115d4bda6d 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_no_stopping_area.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class NoStoppingAreaModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -49,6 +49,6 @@ class NoStoppingAreaModulePlugin : public PluginWrapper -#include -#include +#include +#include +#include #include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -416,4 +416,4 @@ void NoStoppingAreaModule::insertStopPoint( // Insert stop point or replace with zero velocity planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp index 62ec0b88b328e..e7e66775f368e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.hpp @@ -18,9 +18,9 @@ #define EIGEN_MPL2_ONLY #include -#include -#include -#include +#include +#include +#include #include #include @@ -38,7 +38,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using PathIndexWithPose = std::pair; // front index, pose using PathIndexWithPoint2d = std::pair; // front index, point2d @@ -172,6 +172,6 @@ class NoStoppingAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_NO_STOPPING_AREA_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/package.xml b/planning/behavior_velocity_occlusion_spot_module/package.xml index 5817c2203cc5d..ceddd48b045a0 100644 --- a/planning/behavior_velocity_occlusion_spot_module/package.xml +++ b/planning/behavior_velocity_occlusion_spot_module/package.xml @@ -16,9 +16,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs grid_map_ros grid_map_utils diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml index a38900d1a893b..2d85752cf2b3d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ b/planning/behavior_velocity_occlusion_spot_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp index 1170afc490a75..40e5b8f91e6b9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/debug.cpp @@ -15,8 +15,8 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include +#include +#include #include #include @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace { @@ -227,4 +227,4 @@ motion_utils::VirtualWalls OcclusionSpotModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp index c8adb324b9055..427a6f27be3c4 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.cpp @@ -20,7 +20,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace grid_utils { @@ -381,4 +381,4 @@ void denoiseOccupancyGridCV( grid_map::GridMapRosConverter::fromOccupancyGrid(occupancy_grid, "layer", grid_map); } } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp index 710f671df609f..5c8d77fc831df 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/grid_utils.hpp @@ -15,8 +15,8 @@ #ifndef GRID_UTILS_HPP_ #define GRID_UTILS_HPP_ -#include -#include +#include +#include #include #include #include @@ -49,7 +49,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; @@ -118,6 +118,6 @@ void denoiseOccupancyGridCV( grid_map::GridMap & grid_map, const GridParam & param, const bool is_show_debug_window, const int num_iter, const bool use_object_footprints, const bool use_object_ray_casts); } // namespace grid_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // GRID_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp index 64adc112aab2f..b1757c4142212 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "scene_occlusion_spot.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using occlusion_spot_utils::DETECTION_METHOD; using occlusion_spot_utils::PASS_JUDGE; @@ -136,8 +136,9 @@ OcclusionSpotModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OcclusionSpotModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::OcclusionSpotModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp index 0955e4ae9aab5..123d0ef32afc1 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/manager.hpp @@ -18,9 +18,9 @@ #include "occlusion_spot_utils.hpp" #include "scene_occlusion_spot.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModuleManager : public SceneModuleManagerInterface { @@ -63,6 +63,6 @@ class OcclusionSpotModulePlugin : public PluginWrapper -#include +#include +#include #include #include #include @@ -33,7 +33,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; namespace occlusion_spot_utils @@ -485,4 +485,4 @@ std::optional generateOneNotableCollisionFromOcclusionSpo } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp index 2b6d89680cd37..542c869919339 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/occlusion_spot_utils.hpp @@ -17,7 +17,7 @@ #include "grid_utils.hpp" -#include +#include #include #include #include @@ -46,7 +46,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using autoware_perception_msgs::msg::ObjectClassification; using autoware_perception_msgs::msg::PredictedObject; @@ -246,6 +246,6 @@ bool generatePossibleCollisionsFromGridMap( DebugData & debug_data); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // OCCLUSION_SPOT_UTILS_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index 96f497ca47afa..9fe06273ea617 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -16,12 +16,12 @@ #include "occlusion_spot_utils.hpp" -#include +#include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -98,4 +98,4 @@ SafeMotion calculateSafeMotion(const Velocity & v, const double ttv) return sm; } } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp index 8d7e9d2fdedd5..9def3b0938998 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.hpp @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace occlusion_spot_utils { @@ -38,6 +38,6 @@ void applySafeVelocityConsideringPossibleCollision( SafeMotion calculateSafeMotion(const Velocity & v, const double ttv); } // namespace occlusion_spot_utils -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // RISK_PREDICTIVE_BRAKING_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index 1f5ca1bab904e..e5c5eaec346ff 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -17,10 +17,10 @@ #include "occlusion_spot_utils.hpp" #include "risk_predictive_braking.hpp" -#include -#include -#include -#include +#include +#include +#include +#include #include #include @@ -41,7 +41,7 @@ namespace { -namespace utils = behavior_velocity_planner::occlusion_spot_utils; +namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using autoware_perception_msgs::msg::PredictedObject; std::vector extractStuckVehicle( const std::vector & vehicles, const double stop_velocity) @@ -56,7 +56,7 @@ std::vector extractStuckVehicle( } } // namespace -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace utils = occlusion_spot_utils; @@ -201,4 +201,4 @@ bool OcclusionSpotModule::modifyPathVelocity( return true; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp index b7da7c073cbd9..00126a2ebc39a 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.hpp @@ -17,8 +17,8 @@ #include "occlusion_spot_utils.hpp" -#include -#include +#include +#include #include #include @@ -37,7 +37,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OcclusionSpotModule : public SceneModuleInterface { @@ -70,6 +70,6 @@ class OcclusionSpotModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_OCCLUSION_SPOT_HPP_ diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp index bc526959386e9..a21e0333082a9 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_grid_utils.cpp @@ -15,7 +15,7 @@ #include "grid_utils.hpp" #include "utils.hpp" -#include +#include #include #include @@ -38,11 +38,11 @@ struct indexEq } }; -using behavior_velocity_planner::LineString2d; -using behavior_velocity_planner::Point2d; -using behavior_velocity_planner::Polygon2d; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; -using behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; +using autoware::behavior_velocity_planner::LineString2d; +using autoware::behavior_velocity_planner::Point2d; +using autoware::behavior_velocity_planner::Polygon2d; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::OCCUPIED; +using autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::UNKNOWN; namespace bg = boost::geometry; Polygon2d pointsToPoly(const Point2d p0, const Point2d p1, const double radius) diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp index 05e73855f2642..60ef5d62a5b7b 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_occlusion_spot_utils.cpp @@ -16,8 +16,8 @@ #include "occlusion_spot_utils.hpp" #include "utils.hpp" -#include -#include +#include +#include #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/vector3.hpp" @@ -32,8 +32,9 @@ using tier4_planning_msgs::msg::PathWithLaneId; TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; @@ -65,8 +66,9 @@ TEST(calcSlowDownPointsForPossibleCollision, TooManyPossibleCollisions) TEST(calcSlowDownPointsForPossibleCollision, ConsiderSignedOffset) { - using behavior_velocity_planner::occlusion_spot_utils::calcSlowDownPointsForPossibleCollision; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils:: + calcSlowDownPointsForPossibleCollision; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; using std::chrono::duration; using std::chrono::duration_cast; using std::chrono::high_resolution_clock; diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp index 9b1d92b71a111..4a0071be10b2d 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/test_risk_predictive_braking.cpp @@ -23,7 +23,7 @@ TEST(safeMotion, delay_jerk_acceleration) { - namespace utils = behavior_velocity_planner::occlusion_spot_utils; + namespace utils = autoware::behavior_velocity_planner::occlusion_spot_utils; using utils::calculateSafeMotion; /** * @brief check if calculation is correct in below parameter diff --git a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp index f7bb3d4b1ad6f..4b8a7ae3b2f3f 100644 --- a/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp +++ b/planning/behavior_velocity_occlusion_spot_module/test/src/utils.hpp @@ -49,17 +49,18 @@ inline grid_map::GridMap generateGrid(int w, int h, double res) grid_map::GridMap grid{}; grid_map::Length length(w * res, h * res); grid.setGeometry(length, res, grid_map::Position(length.x() / 2.0, length.y() / 2.0)); - grid.add("layer", behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); + grid.add( + "layer", autoware::behavior_velocity_planner::grid_utils::occlusion_cost_value::FREE_SPACE); return grid; } -using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; +using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; inline void generatePossibleCollisions( std::vector & possible_collisions, double x0, double y0, double x, double y, int nb_cols) { - using behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; - using behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::ObstacleInfo; + using autoware::behavior_velocity_planner::occlusion_spot_utils::PossibleCollisionInfo; const double lon = 0.0; // assume col_x = intersection_x const double lat = -1.0; const double velocity = 1.0; diff --git a/planning/behavior_velocity_out_of_lane_module/package.xml b/planning/behavior_velocity_out_of_lane_module/package.xml index d3742ea7d305f..6ea60dd1398f5 100644 --- a/planning/behavior_velocity_out_of_lane_module/package.xml +++ b/planning/behavior_velocity_out_of_lane_module/package.xml @@ -14,9 +14,9 @@ ament_cmake_auto autoware_cmake + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common geometry_msgs lanelet2_extension libboost-dev diff --git a/planning/behavior_velocity_out_of_lane_module/plugins.xml b/planning/behavior_velocity_out_of_lane_module/plugins.xml index 8c18fdce79480..f85eb50367ccc 100644 --- a/planning/behavior_velocity_out_of_lane_module/plugins.xml +++ b/planning/behavior_velocity_out_of_lane_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp index 0c9e6448bb374..5e3877753d203 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/calculate_slowdown_points.hpp @@ -26,7 +26,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief estimate whether ego can decelerate without breaking the deceleration limit @@ -102,5 +102,5 @@ std::optional calculate_slowdown_point( } return std::nullopt; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // CALCULATE_SLOWDOWN_POINTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp index 862ca351a118a..08e66f99e2a0c 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.cpp @@ -18,7 +18,7 @@ #include -namespace behavior_velocity_planner::out_of_lane::debug +namespace autoware::behavior_velocity_planner::out_of_lane::debug { namespace { @@ -184,4 +184,4 @@ void add_range_markers( debug_marker_array.markers.push_back(debug_marker); } -} // namespace behavior_velocity_planner::out_of_lane::debug +} // namespace autoware::behavior_velocity_planner::out_of_lane::debug diff --git a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp index 0802ae78d7a26..05eed6b35c13c 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/debug.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/debug.hpp @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner::out_of_lane::debug +namespace autoware::behavior_velocity_planner::out_of_lane::debug { /// @brief add footprint markers to the given marker array /// @param [inout] debug_marker_array marker array @@ -64,6 +64,6 @@ void add_range_markers( visualization_msgs::msg::MarkerArray & debug_marker_array, const OverlapRanges & ranges, const tier4_planning_msgs::msg::PathWithLaneId & path, const size_t first_path_idx, const double z, const size_t prev_nb); -} // namespace behavior_velocity_planner::out_of_lane::debug +} // namespace autoware::behavior_velocity_planner::out_of_lane::debug #endif // DEBUG_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp index 126c75c2f80b9..62416c8d38ed6 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.cpp @@ -29,7 +29,7 @@ #include #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { double distance_along_path(const EgoData & ego_data, const size_t target_idx) { @@ -380,4 +380,4 @@ std::vector calculate_decisions( return decisions; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp index 4f2b0a6dad89b..27c5b00c96b3f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/decisions.hpp @@ -29,7 +29,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief calculate the distance along the ego path between ego and some target path index /// @param [in] ego_data data related to the ego vehicle @@ -110,6 +110,6 @@ std::optional calculate_decision( /// @return return the calculated decisions to slowdown or stop std::vector calculate_decisions( const DecisionInputs & inputs, const PlannerParam & params, const rclcpp::Logger & logger); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // DECISIONS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp index 80cd106bf52ab..bb352f625580f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.cpp @@ -24,7 +24,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { void cut_predicted_path_beyond_line( autoware_perception_msgs::msg::PredictedPath & predicted_path, @@ -143,4 +143,4 @@ autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( return filtered_objects; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp index be3e8809d2e3d..a12b37e4c7d6d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/filter_predicted_objects.hpp @@ -17,11 +17,11 @@ #include "types.hpp" -#include +#include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief cut a predicted path beyond the given stop line /// @param [inout] predicted_path predicted path to cut @@ -52,6 +52,6 @@ void cut_predicted_path_beyond_red_lights( /// @return filtered predicted objects autoware_perception_msgs::msg::PredictedObjects filter_predicted_objects( const PlannerData & planner_data, const EgoData & ego_data, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // FILTER_PREDICTED_OBJECTS_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp index d136f4a8598f3..48992e46ec74f 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.cpp @@ -24,7 +24,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { tier4_autoware_utils::Polygon2d make_base_footprint( const PlannerParam & p, const bool ignore_offset) @@ -87,4 +87,4 @@ lanelet::BasicPolygon2d calculate_current_ego_footprint( footprint.emplace_back(p.x() + ego_data.pose.position.x, p.y() + ego_data.pose.position.y); return footprint; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp index a573b6ff3981c..1a0d38600fe73 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/footprint.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace out_of_lane { @@ -54,6 +54,6 @@ std::vector calculate_path_footprints( lanelet::BasicPolygon2d calculate_current_ego_footprint( const EgoData & ego_data, const PlannerParam & params, const bool ignore_offset = false); } // namespace out_of_lane -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // FOOTPRINT_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp index 67d8a79a63f03..42df39853f4f5 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.cpp @@ -14,7 +14,7 @@ #include "lanelets_selection.hpp" -#include +#include #include #include @@ -22,7 +22,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { lanelet::ConstLanelets consecutive_lanelets( @@ -127,4 +127,4 @@ lanelet::ConstLanelets calculate_other_lanelets( } return other_lanelets; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp index 87757a0cb2230..8985a49a12853 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/lanelets_selection.hpp @@ -21,7 +21,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief checks if a lanelet is already contained in a vector of lanelets /// @param [in] lanelets vector to check @@ -68,6 +68,6 @@ lanelet::ConstLanelets calculate_other_lanelets( const EgoData & ego_data, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & ignored_lanelets, const route_handler::RouteHandler & route_handler, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // LANELETS_SELECTION_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp index 840054d92252f..61ec1a832bf8e 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.cpp @@ -16,7 +16,7 @@ #include "scene_out_of_lane.hpp" -#include +#include #include #include @@ -25,7 +25,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::getOrDeclareParameter; @@ -100,8 +100,9 @@ OutOfLaneModuleManager::getModuleExpiredFunction( return false; }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::OutOfLaneModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::OutOfLaneModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp index 9da1751576a7f..5ab7126d88d4d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/manager.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene_out_of_lane.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -36,7 +36,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class OutOfLaneModuleManager : public SceneModuleManagerInterface { @@ -61,6 +61,6 @@ class OutOfLaneModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp index 6dfb41ccfbfcb..a081df0b52028 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.cpp @@ -24,7 +24,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { Overlap calculate_overlap( @@ -123,4 +123,4 @@ OverlapRanges calculate_overlapping_ranges( return ranges; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp index 2b0830acc28cc..afb71c11b3ae4 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/overlapping_range.hpp @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief calculate the overlap between the given footprint and lanelet @@ -55,6 +55,6 @@ OverlapRanges calculate_overlapping_ranges( const std::vector & path_footprints, const lanelet::ConstLanelets & path_lanelets, const lanelet::ConstLanelets & lanelets, const PlannerParam & params); -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // OVERLAPPING_RANGE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index 067a5e3dc7698..3f34e8441e8be 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -23,8 +23,8 @@ #include "overlapping_range.hpp" #include "types.hpp" -#include -#include +#include +#include #include #include #include @@ -41,7 +41,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { using visualization_msgs::msg::Marker; @@ -241,4 +241,4 @@ motion_utils::VirtualWalls OutOfLaneModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp index 1d51c45c6afd1..38101d6a9ba1d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.hpp @@ -17,7 +17,7 @@ #include "types.hpp" -#include +#include #include #include @@ -31,7 +31,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { class OutOfLaneModule : public SceneModuleInterface { @@ -60,6 +60,6 @@ class OutOfLaneModule : public SceneModuleInterface // Debug mutable DebugData debug_data_; }; -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // SCENE_OUT_OF_LANE_HPP_ diff --git a/planning/behavior_velocity_out_of_lane_module/src/types.hpp b/planning/behavior_velocity_out_of_lane_module/src/types.hpp index cb2945f8b32b8..f1c48a6e96d17 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/types.hpp +++ b/planning/behavior_velocity_out_of_lane_module/src/types.hpp @@ -31,7 +31,7 @@ #include #include -namespace behavior_velocity_planner::out_of_lane +namespace autoware::behavior_velocity_planner::out_of_lane { /// @brief parameters for the "out of lane" module struct PlannerParam @@ -230,6 +230,6 @@ struct DebugData } }; -} // namespace behavior_velocity_planner::out_of_lane +} // namespace autoware::behavior_velocity_planner::out_of_lane #endif // TYPES_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/package.xml b/planning/behavior_velocity_speed_bump_module/package.xml index 9b1de66c95c92..62e215951757d 100644 --- a/planning/behavior_velocity_speed_bump_module/package.xml +++ b/planning/behavior_velocity_speed_bump_module/package.xml @@ -15,8 +15,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_speed_bump_module/plugins.xml b/planning/behavior_velocity_speed_bump_module/plugins.xml index 506d25669f8cf..48287a7f94b4b 100644 --- a/planning/behavior_velocity_speed_bump_module/plugins.xml +++ b/planning/behavior_velocity_speed_bump_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_speed_bump_module/src/debug.cpp b/planning/behavior_velocity_speed_bump_module/src/debug.cpp index 7409c24e7e8bf..acbe3e05debaa 100644 --- a/planning/behavior_velocity_speed_bump_module/src/debug.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -23,7 +23,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::createSlowDownVirtualWallMarker; using tier4_autoware_utils::appendMarkerArray; @@ -120,4 +120,4 @@ visualization_msgs::msg::MarkerArray SpeedBumpModule::createDebugMarkerArray() return debug_marker_array; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.cpp b/planning/behavior_velocity_speed_bump_module/src/manager.cpp index fcd000699b7ff..42f10586d8fec 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::autoware::SpeedBump; using tier4_autoware_utils::getOrDeclareParameter; @@ -80,8 +80,9 @@ SpeedBumpModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::SpeedBumpModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::SpeedBumpModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_speed_bump_module/src/manager.hpp b/planning/behavior_velocity_speed_bump_module/src/manager.hpp index 40fcfdd081c2e..41b9a593dde95 100644 --- a/planning/behavior_velocity_speed_bump_module/src/manager.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -27,7 +27,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class SpeedBumpModuleManager : public SceneModuleManagerInterface { @@ -49,6 +49,6 @@ class SpeedBumpModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.cpp b/planning/behavior_velocity_speed_bump_module/src/scene.cpp index dc944cc0c292b..0ca4e0731b155 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.cpp @@ -20,7 +20,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using motion_utils::calcSignedArcLength; using tier4_autoware_utils::createPoint; @@ -169,4 +169,4 @@ bool SpeedBumpModule::applySlowDownSpeed( output.points, slow_start_point_idx, slow_end_point_idx, speed_bump_speed); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/scene.hpp b/planning/behavior_velocity_speed_bump_module/src/scene.hpp index f227366127046..ffbcf5fdf9325 100644 --- a/planning/behavior_velocity_speed_bump_module/src/scene.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/scene.hpp @@ -17,14 +17,14 @@ #include "util.hpp" -#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_planning_msgs::msg::PathWithLaneId; @@ -81,6 +81,6 @@ class SpeedBumpModule : public SceneModuleInterface float speed_bump_slow_down_speed_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_speed_bump_module/src/util.cpp b/planning/behavior_velocity_speed_bump_module/src/util.cpp index d1533d7028dcf..ca4e7e379d1ea 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.cpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.cpp @@ -17,7 +17,7 @@ #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "motion_utils/trajectory/trajectory.hpp" -#include +#include #include #include @@ -32,7 +32,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -172,4 +172,4 @@ float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float spee return std::clamp(speed, p2.y, p1.y); } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_speed_bump_module/src/util.hpp b/planning/behavior_velocity_speed_bump_module/src/util.hpp index 2cd408735fd61..72bad3db86f73 100644 --- a/planning/behavior_velocity_speed_bump_module/src/util.hpp +++ b/planning/behavior_velocity_speed_bump_module/src/util.hpp @@ -35,7 +35,7 @@ #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -70,6 +70,6 @@ std::optional insertPointWithOffset( // returns y (speed) for y=mx+b float calcSlowDownSpeed(const Point32 & p1, const Point32 & p2, const float speed_bump_height); -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // UTIL_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/package.xml b/planning/behavior_velocity_stop_line_module/package.xml index c71ecb83098fb..3132f60d1f342 100644 --- a/planning/behavior_velocity_stop_line_module/package.xml +++ b/planning/behavior_velocity_stop_line_module/package.xml @@ -18,8 +18,8 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_stop_line_module/plugins.xml b/planning/behavior_velocity_stop_line_module/plugins.xml index 51fb225fbebad..4b8ce9852fcec 100644 --- a/planning/behavior_velocity_stop_line_module/plugins.xml +++ b/planning/behavior_velocity_stop_line_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_stop_line_module/src/debug.cpp b/planning/behavior_velocity_stop_line_module/src/debug.cpp index f63bdca5068a2..bfcd5a5828556 100644 --- a/planning/behavior_velocity_stop_line_module/src/debug.cpp +++ b/planning/behavior_velocity_stop_line_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using tier4_autoware_utils::appendMarkerArray; using tier4_autoware_utils::createMarkerColor; @@ -111,4 +111,4 @@ motion_utils::VirtualWalls StopLineModule::createVirtualWalls() } return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/manager.cpp b/planning/behavior_velocity_stop_line_module/src/manager.cpp index b37b3b119178c..80f48fa65efaf 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.cpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.cpp @@ -21,7 +21,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficSign; using tier4_autoware_utils::getOrDeclareParameter; @@ -102,8 +102,9 @@ StopLineModuleManager::getModuleExpiredFunction( }; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::StopLineModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::StopLineModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_stop_line_module/src/manager.hpp b/planning/behavior_velocity_stop_line_module/src/manager.hpp index af9dbaa083c33..eca5132a75c5f 100644 --- a/planning/behavior_velocity_stop_line_module/src/manager.hpp +++ b/planning/behavior_velocity_stop_line_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -30,7 +30,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using StopLineWithLaneId = std::pair; @@ -62,6 +62,6 @@ class StopLineModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_stop_line_module/src/scene.cpp b/planning/behavior_velocity_stop_line_module/src/scene.cpp index 976acd98fe42b..1bf777338e43c 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.cpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.cpp @@ -14,14 +14,14 @@ #include "scene.hpp" -#include -#include +#include +#include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -177,4 +177,4 @@ geometry_msgs::msg::Point StopLineModule::getCenterOfStopLine( center_point.z = (stop_line[0].z() + stop_line[1].z()) / 2.0; return center_point; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_stop_line_module/src/scene.hpp b/planning/behavior_velocity_stop_line_module/src/scene.hpp index 70c67df623c85..0ae0a885f036f 100644 --- a/planning/behavior_velocity_stop_line_module/src/scene.hpp +++ b/planning/behavior_velocity_stop_line_module/src/scene.hpp @@ -24,16 +24,16 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include -#include +#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class StopLineModule : public SceneModuleInterface { @@ -106,6 +106,6 @@ class StopLineModule : public SceneModuleInterface // Debug DebugData debug_data_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/package.xml b/planning/behavior_velocity_traffic_light_module/package.xml index ff7b5a269db32..067dcaca6a638 100644 --- a/planning/behavior_velocity_traffic_light_module/package.xml +++ b/planning/behavior_velocity_traffic_light_module/package.xml @@ -19,9 +19,9 @@ eigen3_cmake_module autoware_adapi_v1_msgs + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/behavior_velocity_traffic_light_module/plugins.xml b/planning/behavior_velocity_traffic_light_module/plugins.xml index b65cc66c5c232..9b5e84fff982d 100644 --- a/planning/behavior_velocity_traffic_light_module/plugins.xml +++ b/planning/behavior_velocity_traffic_light_module/plugins.xml @@ -1,3 +1,3 @@ - + diff --git a/planning/behavior_velocity_traffic_light_module/src/debug.cpp b/planning/behavior_velocity_traffic_light_module/src/debug.cpp index b3dedaa29d6ad..7ceded9c91934 100644 --- a/planning/behavior_velocity_traffic_light_module/src/debug.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/debug.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include #include @@ -24,7 +24,7 @@ #include #endif -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { visualization_msgs::msg::MarkerArray TrafficLightModule::createDebugMarkerArray() { @@ -54,4 +54,4 @@ motion_utils::VirtualWalls TrafficLightModule::createVirtualWalls() return virtual_walls; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.cpp b/planning/behavior_velocity_traffic_light_module/src/manager.cpp index 01c3acf84cd63..ef5f8b2b36016 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.cpp @@ -14,7 +14,7 @@ #include "manager.hpp" -#include +#include #include #include @@ -24,7 +24,7 @@ #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { using lanelet::TrafficLight; using tier4_autoware_utils::getOrDeclareParameter; @@ -188,8 +188,9 @@ bool TrafficLightModuleManager::hasSameTrafficLight( return false; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #include PLUGINLIB_EXPORT_CLASS( - behavior_velocity_planner::TrafficLightModulePlugin, behavior_velocity_planner::PluginInterface) + autoware::behavior_velocity_planner::TrafficLightModulePlugin, + autoware::behavior_velocity_planner::PluginInterface) diff --git a/planning/behavior_velocity_traffic_light_module/src/manager.hpp b/planning/behavior_velocity_traffic_light_module/src/manager.hpp index 97340f8592a7d..c40056a0ba10c 100644 --- a/planning/behavior_velocity_traffic_light_module/src/manager.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/manager.hpp @@ -17,9 +17,9 @@ #include "scene.hpp" -#include -#include -#include +#include +#include +#include #include #include @@ -28,7 +28,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC { @@ -65,6 +65,6 @@ class TrafficLightModulePlugin : public PluginWrapper { }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // MANAGER_HPP_ diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.cpp b/planning/behavior_velocity_traffic_light_module/src/scene.cpp index c886578dc65e4..3ec597f1de249 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.cpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.cpp @@ -14,7 +14,7 @@ #include "scene.hpp" -#include +#include #include #include @@ -34,7 +34,7 @@ #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { namespace bg = boost::geometry; @@ -432,4 +432,4 @@ tier4_planning_msgs::msg::PathWithLaneId TrafficLightModule::insertStopPose( return modified_path; } -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner diff --git a/planning/behavior_velocity_traffic_light_module/src/scene.hpp b/planning/behavior_velocity_traffic_light_module/src/scene.hpp index 8385a1210d421..220145a2c7163 100644 --- a/planning/behavior_velocity_traffic_light_module/src/scene.hpp +++ b/planning/behavior_velocity_traffic_light_module/src/scene.hpp @@ -23,15 +23,15 @@ #define EIGEN_MPL2_ONLY #include #include -#include -#include +#include +#include #include #include #include #include -namespace behavior_velocity_planner +namespace autoware::behavior_velocity_planner { class TrafficLightModule : public SceneModuleInterface { @@ -129,6 +129,6 @@ class TrafficLightModule : public SceneModuleInterface // Traffic Light State TrafficSignal looking_tl_state_; }; -} // namespace behavior_velocity_planner +} // namespace autoware::behavior_velocity_planner #endif // SCENE_HPP_ diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp index add6b5ef392ea..87aa4ac483feb 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware_motion_velocity_planner_common/planner_data.hpp @@ -81,7 +81,7 @@ struct PlannerData tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother - std::shared_ptr velocity_smoother_{}; + std::shared_ptr velocity_smoother_{}; // parameters vehicle_info_util::VehicleInfo vehicle_info_; diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml index 2f4e7241d60ee..a115d28ee6d86 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_common/package.xml @@ -15,10 +15,10 @@ autoware_cmake eigen3_cmake_module + autoware_behavior_velocity_planner_common autoware_perception_msgs autoware_planning_msgs autoware_velocity_smoother - behavior_velocity_planner_common eigen geometry_msgs lanelet2_extension diff --git a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp index 4d152afa250b7..262c32fe1ea47 100644 --- a/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp +++ b/planning/motion_velocity_planner/autoware_motion_velocity_planner_node/src/node.cpp @@ -241,7 +241,7 @@ void MotionVelocityPlannerNode::on_acceleration( void MotionVelocityPlannerNode::set_velocity_smoother_params() { planner_data_.velocity_smoother_ = - std::make_shared(*this); + std::make_shared(*this); } void MotionVelocityPlannerNode::on_lanelet_map( @@ -399,7 +399,7 @@ autoware::motion_velocity_planner::TrajectoryPoints MotionVelocityPlannerNode::s traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest); if (external_v_limit) { - autoware_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( + autoware::velocity_smoother::trajectory_utils::applyMaximumVelocityLimit( traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed); } return traj_smoothed; diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 08cee3d58152e..8b338e9d0cd9a 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -218,3 +218,15 @@ lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + stop: + type_specified_params: + labels: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 052e7bb721592..34ae20980d3fd 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -113,14 +113,15 @@ struct StopObstacle : public TargetObstacleInterface { StopObstacle( const std::string & arg_uuid, const rclcpp::Time & arg_stamp, - const geometry_msgs::msg::Pose & arg_pose, const Shape & arg_shape, - const double arg_lon_velocity, const double arg_lat_velocity, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity, const geometry_msgs::msg::Point arg_collision_point, const double arg_dist_to_collide_on_decimated_traj) : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), shape(arg_shape), collision_point(arg_collision_point), - dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj) + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) { } Shape shape; @@ -129,6 +130,7 @@ struct StopObstacle : public TargetObstacleInterface // calculateMarginFromObstacleOnCurve() and should be removed as it can be // replaced by ”dist_to_collide_on_decimated_traj” double dist_to_collide_on_decimated_traj; + ObjectClassification classification; }; struct CruiseObstacle : public TargetObstacleInterface diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index fd65446408db1..3aea741e3b2f4 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -272,8 +272,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_driving_forward_{true}; bool enable_slow_down_planning_{false}; - // previous closest obstacle - std::shared_ptr prev_closest_stop_obstacle_ptr_{nullptr}; + std::vector prev_closest_stop_obstacles_{}; std::unique_ptr logger_configure_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 416c36b116059..7a060657e16af 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -23,6 +23,8 @@ #include "tier4_autoware_utils/ros/update_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include +#include #include #include #include @@ -42,7 +44,8 @@ class PlannerInterface vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), - slow_down_param_(SlowDownParam(node)) + slow_down_param_(SlowDownParam(node)), + stop_param_(StopParam(node, longitudinal_info)) { stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = @@ -91,6 +94,7 @@ class PlannerInterface updateCommonParam(parameters); updateCruiseParam(parameters); slow_down_param_.onParam(parameters); + stop_param_.onParam(parameters, longitudinal_info_); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -333,6 +337,84 @@ class PlannerInterface double lpf_gain_dist_to_slow_down; }; SlowDownParam slow_down_param_; + struct StopParam + { + struct ObstacleSpecificParams + { + double limit_min_acc; + double sudden_object_acc_threshold; + double sudden_object_dist_threshold; + bool abandon_to_stop; + }; + const std::unordered_map types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map type_specified_param_list; + explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + std::vector obstacle_labels{"default"}; + obstacle_labels = + node.declare_parameter>(param_prefix + "labels", obstacle_labels); + + for (const auto & type_str : obstacle_labels) { + if (type_str != "default") { + ObstacleSpecificParams param{ + node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_acc_threshold"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_dist_threshold"), + node.declare_parameter(param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + type_specified_param_list.emplace(type_str, param); + } + } + } + void onParam( + const std::vector & parameters, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + for (auto & [type_str, param] : type_specified_param_list) { + if (type_str == "default") { + continue; + } + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_acc_threshold", + param.sudden_object_acc_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_dist_threshold", + param.sudden_object_dist_threshold); + tier4_autoware_utils::updateParam( + parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + } + } + std::string getParamType(const ObjectClassification label) + { + const auto type_str = types_maps.at(label.label); + if (type_specified_param_list.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObstacleSpecificParams getParam(const ObjectClassification label) + { + return type_specified_param_list.at(getParamType(label)); + } + }; + StopParam stop_param_; double moving_object_speed_threshold; double moving_object_hysteresis_range; std::vector prev_slow_down_output_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 3b4ab577c2988..48bab514cbf77 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -36,8 +36,7 @@ PoseWithStamp getCurrentObjectPose( const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const bool use_prediction); -std::optional getClosestStopObstacle( - const std::vector & stop_obstacles); +std::vector getClosestStopObstacles(const std::vector & stop_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c5f0c73a13b78..41ae29b7d470f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -1253,9 +1253,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( } const auto [tangent_vel, normal_vel] = projectObstacleVelocityToTrajectory(traj_points, obstacle); - return StopObstacle{ - obstacle.uuid, obstacle.stamp, obstacle.pose, obstacle.shape, - tangent_vel, normal_vel, collision_point->first, collision_point->second}; + return StopObstacle{obstacle.uuid, obstacle.stamp, obstacle.classification, + obstacle.pose, obstacle.shape, tangent_vel, + normal_vel, collision_point->first, collision_point->second}; } std::optional ObstacleCruisePlannerNode::createSlowDownObstacle( @@ -1386,50 +1386,36 @@ void ObstacleCruisePlannerNode::checkConsistency( const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, std::vector & stop_obstacles) { - const auto current_closest_stop_obstacle = - obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - - if (!prev_closest_stop_obstacle_ptr_) { - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_obstacles_) { + const auto predicted_object_itr = std::find_if( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [&prev_closest_stop_obstacle](const PredictedObject & po) { + return tier4_autoware_utils::toHexString(po.object_id) == prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (predicted_object_itr == predicted_objects.objects.end()) { + continue; } - return; - } - const auto predicted_object_itr = std::find_if( - predicted_objects.objects.begin(), predicted_objects.objects.end(), - [&](PredictedObject predicted_object) { - return tier4_autoware_utils::toHexString(predicted_object.object_id) == - prev_closest_stop_obstacle_ptr_->uuid; - }); - // If previous closest obstacle disappear from the perception result, do nothing anymore. - if (predicted_object_itr == predicted_objects.objects.end()) { - return; - } - - const auto is_disappeared_from_stop_obstacle = std::none_of( - stop_obstacles.begin(), stop_obstacles.end(), - [&](const auto & obstacle) { return obstacle.uuid == prev_closest_stop_obstacle_ptr_->uuid; }); - if (is_disappeared_from_stop_obstacle) { - // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" - // condition is satisfied - const double elapsed_time = (current_time - prev_closest_stop_obstacle_ptr_->stamp).seconds(); - if ( - predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < - behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && - elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { - stop_obstacles.push_back(*prev_closest_stop_obstacle_ptr_); - return; + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && + elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } } } - if (current_closest_stop_obstacle) { - prev_closest_stop_obstacle_ptr_ = - std::make_shared(*current_closest_stop_obstacle); - } else { - prev_closest_stop_obstacle_ptr_ = nullptr; - } + prev_closest_stop_obstacles_ = obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); } bool ObstacleCruisePlannerNode::isObstacleCrossing( diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index f7ce218cf3ccf..6e1de97378d16 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -259,129 +259,171 @@ std::vector PlannerInterface::generateStopTrajectory( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, "stop planning"); - // Get Closest Stop Obstacle - const auto closest_stop_obstacle = obstacle_cruise_utils::getClosestStopObstacle(stop_obstacles); - if (!closest_stop_obstacle) { - // delete marker - const auto markers = - motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - - prev_stop_distance_info_ = std::nullopt; - return planner_data.traj_points; - } - - const auto ego_segment_idx = - ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); - const double dist_to_collide_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + - closest_stop_obstacle->dist_to_collide_on_decimated_traj; - - const double margin_from_obstacle_considering_behavior_module = [&]() { - const double margin_from_obstacle = - calculateMarginFromObstacleOnCurve(planner_data, *closest_stop_obstacle); - // Use terminal margin (terminal_safe_distance_margin) for obstacle stop - const auto ref_traj_length = motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.traj_points.size() - 1); - if (dist_to_collide_on_ref_traj > ref_traj_length) { - return longitudinal_info_.terminal_safe_distance_margin; - } - - // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin - // we set closest_obstacle_stop_distance to closest_behavior_stop_distance - const auto closest_behavior_stop_idx = - motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); - if (closest_behavior_stop_idx) { - const double closest_behavior_stop_dist_on_ref_traj = - motion_utils::calcSignedArcLength(planner_data.traj_points, 0, *closest_behavior_stop_idx); - const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - - (dist_to_collide_on_ref_traj - margin_from_obstacle); - if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { - return min_behavior_stop_margin_; + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_margin{}; + + const auto closest_stop_obstacles = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + const double dist_to_collide_on_ref_traj = + motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + const double desired_margin = [&]() { + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); + // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + const auto ref_traj_length = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + return longitudinal_info_.terminal_safe_distance_margin; } - } - return margin_from_obstacle; - }(); - // Generate Output Trajectory - const auto [zero_vel_dist, will_collide_with_obstacle] = [&]() { - double candidate_zero_vel_dist = - std::max(0.0, dist_to_collide_on_ref_traj - margin_from_obstacle_considering_behavior_module); + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain + // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const auto closest_behavior_stop_idx = + motion_utils::searchZeroVelocityIndex(planner_data.traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - + (dist_to_collide_on_ref_traj - margin_from_obstacle); + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { + return min_behavior_stop_margin_; + } + } + return margin_from_obstacle; + }(); - // Check feasibility to stop + // calc stop point against the obstacle + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); if (suppress_sudden_obstacle_stop_) { - // Calculate feasible stop margin (Check the feasibility) - const double feasible_stop_dist = - calcMinimumDistanceToStop( - planner_data.ego_vel, longitudinal_info_.limit_max_accel, - longitudinal_info_.limit_min_accel) + - motion_utils::calcSignedArcLength( - planner_data.traj_points, 0, planner_data.ego_pose.position); + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_param_.getParamType(stop_obstacle.classification) == "default") { + return longitudinal_info_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), + stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return longitudinal_info_.limit_min_accel; + } + if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_param_.types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_param_.getParam(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + continue; + } - if (candidate_zero_vel_dist < feasible_stop_dist) { - candidate_zero_vel_dist = feasible_stop_dist; - return std::make_pair(candidate_zero_vel_dist, true); + const double acceptable_stop_pos = + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; } } - // Hold previous stop distance if necessary - if ( - std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && - prev_stop_distance_info_) { - // NOTE: We assume that the current trajectory's front point is ahead of the previous - // trajectory's front point. - const size_t traj_front_point_prev_seg_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prev_stop_distance_info_->first, planner_data.traj_points.front().pose); - const double diff_dist_front_points = motion_utils::calcSignedArcLength( - prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, - traj_front_point_prev_seg_idx); - - const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); if ( - std::abs(prev_zero_vel_dist - candidate_zero_vel_dist) < - longitudinal_info_.hold_stop_distance_threshold) { - candidate_zero_vel_dist = prev_zero_vel_dist; + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; } } - return std::make_pair(candidate_zero_vel_dist, false); - }(); + determined_zero_vel_dist = candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_margin = desired_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return planner_data.traj_points; + } + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + longitudinal_info_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } // Insert stop point auto output_traj_points = planner_data.traj_points; - const auto zero_vel_idx = motion_utils::insertStopPoint(0, zero_vel_dist, output_traj_points); + const auto zero_vel_idx = + motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); if (zero_vel_idx) { // virtual wall marker for stop obstacle const auto markers = motion_utils::createStopVirtualWallMarker( output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, abs_ego_offset, "", planner_data.is_driving_forward); tier4_autoware_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); - debug_data_ptr_->obstacles_to_stop.push_back(*closest_stop_obstacle); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); // Publish Stop Reason const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; const auto stop_reasons_msg = - makeStopReasonArray(planner_data, stop_pose, *closest_stop_obstacle); + makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle); stop_reasons_pub_->publish(stop_reasons_msg); velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose)); - // Publish if ego vehicle collides with the obstacle with a limit acceleration + // Publish if ego vehicle will over run against the stop point with a limit acceleration + + const bool will_over_run = determined_zero_vel_dist.value() > + motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + determined_stop_obstacle->dist_to_collide_on_decimated_traj + + determined_desired_margin.value() + 0.1; const auto stop_speed_exceeded_msg = - createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + createStopSpeedExceededMsg(planner_data.current_time, will_over_run); stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); - prev_stop_distance_info_ = std::make_pair(output_traj_points, zero_vel_dist); + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); } stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, - closest_stop_obstacle->dist_to_collide_on_decimated_traj); + determined_stop_obstacle->dist_to_collide_on_decimated_traj); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); - + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); stop_planning_debug_info_.set( - StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, - margin_from_obstacle_considering_behavior_module); + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value()); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 6b32ccafc7f42..6c1b3999e40c1 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -95,16 +95,21 @@ PoseWithStamp getCurrentObjectPose( return PoseWithStamp{obj_base_time, *interpolated_pose}; } -std::optional getClosestStopObstacle(const std::vector & stop_obstacles) +std::vector getClosestStopObstacles(const std::vector & stop_obstacles) { - std::optional candidate_obstacle = std::nullopt; + std::vector candidates{}; for (const auto & stop_obstacle : stop_obstacles) { - if ( - !candidate_obstacle || stop_obstacle.dist_to_collide_on_decimated_traj < - candidate_obstacle->dist_to_collide_on_decimated_traj) { - candidate_obstacle = stop_obstacle; + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; } } - return candidate_obstacle; + return candidates; } } // namespace obstacle_cruise_utils diff --git a/planning/planning_validator/CMakeLists.txt b/planning/planning_validator/CMakeLists.txt index 455dded2d7e32..da780cfdebdcf 100644 --- a/planning/planning_validator/CMakeLists.txt +++ b/planning/planning_validator/CMakeLists.txt @@ -1,22 +1,22 @@ cmake_minimum_required(VERSION 3.22) -project(planning_validator) +project(autoware_planning_validator) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(planning_validator_helpers SHARED +ament_auto_add_library(autoware_planning_validator_helpers SHARED src/utils.cpp src/debug_marker.cpp ) # planning validator -ament_auto_add_library(planning_validator_component SHARED - include/planning_validator/planning_validator.hpp +ament_auto_add_library(autoware_planning_validator_component SHARED + include/autoware_planning_validator/planning_validator.hpp src/planning_validator.cpp ) -target_link_libraries(planning_validator_component planning_validator_helpers) -rclcpp_components_register_node(planning_validator_component - PLUGIN "planning_validator::PlanningValidator" +target_link_libraries(autoware_planning_validator_component autoware_planning_validator_helpers) +rclcpp_components_register_node(autoware_planning_validator_component + PLUGIN "autoware::planning_validator::PlanningValidator" EXECUTABLE planning_validator_node ) @@ -25,7 +25,7 @@ ament_auto_add_library(invalid_trajectory_publisher_node SHARED src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp ) rclcpp_components_register_node(invalid_trajectory_publisher_node - PLUGIN "planning_validator::InvalidTrajectoryPublisherNode" + PLUGIN "autoware::planning_validator::InvalidTrajectoryPublisherNode" EXECUTABLE invalid_trajectory_publisher ) @@ -37,29 +37,29 @@ rosidl_generate_interfaces( # to use a message defined in the same package if(${rosidl_cmake_VERSION} VERSION_LESS 2.5.0) - rosidl_target_interfaces(planning_validator_component + rosidl_target_interfaces(autoware_planning_validator_component ${PROJECT_NAME} "rosidl_typesupport_cpp") else() rosidl_get_typesupport_target( cpp_typesupport_target ${PROJECT_NAME} "rosidl_typesupport_cpp") - target_link_libraries(planning_validator_component "${cpp_typesupport_target}") + target_link_libraries(autoware_planning_validator_component "${cpp_typesupport_target}") endif() if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_planning_validator + ament_add_ros_isolated_gtest(test_autoware_planning_validator test/src/test_main.cpp test/src/test_planning_validator_functions.cpp test/src/test_planning_validator_helper.cpp test/src/test_planning_validator_pubsub.cpp - test/src/test_${PROJECT_NAME}_node_interface.cpp + test/src/test_planning_validator_node_interface.cpp ) - ament_target_dependencies(test_planning_validator + ament_target_dependencies(test_autoware_planning_validator rclcpp autoware_planning_msgs ) - target_link_libraries(test_planning_validator - planning_validator_component + target_link_libraries(test_autoware_planning_validator + autoware_planning_validator_component ) endif() diff --git a/planning/planning_validator/README.md b/planning/planning_validator/README.md index 9d70e7f78a5bb..92c9b86c8b31c 100644 --- a/planning/planning_validator/README.md +++ b/planning/planning_validator/README.md @@ -1,8 +1,8 @@ # Planning Validator -The `planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". +The `autoware_planning_validator` is a module that checks the validity of a trajectory before it is published. The status of the validation can be viewed in the `/diagnostics` and `/validation_status` topics. When an invalid trajectory is detected, the `autoware_planning_validator` will process the trajectory following the selected option: "0. publish the trajectory as it is", "1. stop publishing the trajectory", "2. publish the last validated trajectory". -![planning_validator](./image/planning_validator.drawio.svg) +![autoware_planning_validator](./image/planning_validator.drawio.svg) ## Supported features @@ -29,7 +29,7 @@ The following features are to be implemented. ### Inputs -The `planning_validator` takes in the following inputs: +The `autoware_planning_validator` takes in the following inputs: | Name | Type | Description | | -------------------- | --------------------------------- | ---------------------------------------------- | @@ -48,7 +48,7 @@ It outputs the following: ## Parameters -The following parameters can be set for the `planning_validator`: +The following parameters can be set for the `autoware_planning_validator`: ### System parameters diff --git a/planning/planning_validator/config/planning_validator_plotjugler_config.xml b/planning/planning_validator/config/planning_validator_plotjugler_config.xml index d6afc0b017d32..04d5d4d8a0e38 100644 --- a/planning/planning_validator/config/planning_validator_plotjugler_config.xml +++ b/planning/planning_validator/config/planning_validator_plotjugler_config.xml @@ -9,21 +9,21 @@ - + - + - + @@ -32,21 +32,21 @@ - + - + - + @@ -55,21 +55,21 @@ - + - + - + @@ -78,7 +78,7 @@ - + @@ -86,23 +86,23 @@ - - - - - + + + + + - - - - - - + + + + + + diff --git a/planning/planning_validator/image/planning_validator.drawio.svg b/planning/planning_validator/image/planning_validator.drawio.svg index 070c1a6d68b81..0a2267cba159e 100644 --- a/planning/planning_validator/image/planning_validator.drawio.svg +++ b/planning/planning_validator/image/planning_validator.drawio.svg @@ -38,12 +38,12 @@ >
- planning_validator + autoware_planning_validator
- planning_validator + autoware_planning_validator diff --git a/planning/planning_validator/include/planning_validator/debug_marker.hpp b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/debug_marker.hpp rename to planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp index 4671a0b0d7297..955ba5bbf64ab 100644 --- a/planning/planning_validator/include/planning_validator/debug_marker.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/debug_marker.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ -#define PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ #include @@ -57,4 +57,4 @@ class PlanningValidatorDebugMarkerPublisher } }; -#endif // PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__DEBUG_MARKER_HPP_ diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp similarity index 91% rename from planning/planning_validator/include/planning_validator/planning_validator.hpp rename to planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp index fed6e161c4d90..113c70fecb62c 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/planning_validator.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#define PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/msg/planning_validator_status.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/msg/planning_validator_status.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -33,14 +33,14 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_validator::msg::PlanningValidatorStatus; using diagnostic_updater::DiagnosticStatusWrapper; using diagnostic_updater::Updater; using nav_msgs::msg::Odometry; -using planning_validator::msg::PlanningValidatorStatus; using tier4_autoware_utils::StopWatch; using tier4_debug_msgs::msg::Float64Stamped; @@ -141,6 +141,6 @@ class PlanningValidator : public rclcpp::Node StopWatch stop_watch_; }; -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_ diff --git a/planning/planning_validator/include/planning_validator/utils.hpp b/planning/planning_validator/include/autoware_planning_validator/utils.hpp similarity index 90% rename from planning/planning_validator/include/planning_validator/utils.hpp rename to planning/planning_validator/include/autoware_planning_validator/utils.hpp index 11eaf4f37f400..1c943f5a60e08 100644 --- a/planning/planning_validator/include/planning_validator/utils.hpp +++ b/planning/planning_validator/include/autoware_planning_validator/utils.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef PLANNING_VALIDATOR__UTILS_HPP_ -#define PLANNING_VALIDATOR__UTILS_HPP_ +#ifndef AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ +#define AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -65,6 +65,6 @@ bool checkFinite(const TrajectoryPoint & point); void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal); -} // namespace planning_validator +} // namespace autoware::planning_validator -#endif // PLANNING_VALIDATOR__UTILS_HPP_ +#endif // AUTOWARE_PLANNING_VALIDATOR__UTILS_HPP_ diff --git a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml index 3f73e0f462cc2..a1cdc7c7ecc55 100644 --- a/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml +++ b/planning/planning_validator/launch/invalid_trajectory_publisher.launch.xml @@ -2,7 +2,7 @@ - + diff --git a/planning/planning_validator/launch/planning_validator.launch.xml b/planning/planning_validator/launch/planning_validator.launch.xml index 81573adb4dc24..fa1475a0787e4 100644 --- a/planning/planning_validator/launch/planning_validator.launch.xml +++ b/planning/planning_validator/launch/planning_validator.launch.xml @@ -1,9 +1,9 @@ - + - + diff --git a/planning/planning_validator/package.xml b/planning/planning_validator/package.xml index 5a413f27dc7a3..7fe57e68928d9 100644 --- a/planning/planning_validator/package.xml +++ b/planning/planning_validator/package.xml @@ -1,9 +1,9 @@ - planning_validator + autoware_planning_validator 0.1.0 - ros node for planning_validator + ros node for autoware_planning_validator Takamasa Horibe Kosuke Takeuchi Apache License 2.0 diff --git a/planning/planning_validator/src/debug_marker.cpp b/planning/planning_validator/src/debug_marker.cpp index 82117d199c36e..413611a72b5ab 100644 --- a/planning/planning_validator/src/debug_marker.cpp +++ b/planning/planning_validator/src/debug_marker.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/debug_marker.hpp" #include #include @@ -90,7 +90,7 @@ void PlanningValidatorDebugMarkerPublisher::pushVirtualWall(const geometry_msgs: { const auto now = node_->get_clock()->now(); const auto stop_wall_marker = - motion_utils::createStopVirtualWallMarker(pose, "planning_validator", now, 0); + motion_utils::createStopVirtualWallMarker(pose, "autoware_planning_validator", now, 0); tier4_autoware_utils::appendMarkerArray(stop_wall_marker, &marker_array_virtual_wall_, now); } diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp index 6cca99cd9f14c..645d6c92e3698 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.cpp @@ -18,7 +18,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { InvalidTrajectoryPublisherNode::InvalidTrajectoryPublisherNode( const rclcpp::NodeOptions & node_options) @@ -72,7 +72,7 @@ void InvalidTrajectoryPublisherNode::onCurrentTrajectory(const Trajectory::Const traj_sub_.reset(); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::InvalidTrajectoryPublisherNode) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::InvalidTrajectoryPublisherNode) diff --git a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp index 1d1e47ad10407..0025d759217bf 100644 --- a/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp +++ b/planning/planning_validator/src/invalid_trajectory_publisher/invalid_trajectory_publisher.hpp @@ -21,7 +21,7 @@ #include -namespace planning_validator +namespace autoware::planning_validator { using autoware_planning_msgs::msg::Trajectory; using autoware_planning_msgs::msg::TrajectoryPoint; @@ -41,6 +41,6 @@ class InvalidTrajectoryPublisherNode : public rclcpp::Node Trajectory::ConstSharedPtr current_trajectory_ = nullptr; }; -} // namespace planning_validator +} // namespace autoware::planning_validator #endif // INVALID_TRAJECTORY_PUBLISHER__INVALID_TRAJECTORY_PUBLISHER_HPP_ diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index d14b1d9ff37d2..aac50e24c5d5c 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -23,7 +23,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using diagnostic_msgs::msg::DiagnosticStatus; @@ -584,7 +584,7 @@ void PlanningValidator::displayStatus() warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!"); } -} // namespace planning_validator +} // namespace autoware::planning_validator #include -RCLCPP_COMPONENTS_REGISTER_NODE(planning_validator::PlanningValidator) +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::planning_validator::PlanningValidator) diff --git a/planning/planning_validator/src/utils.cpp b/planning/planning_validator/src/utils.cpp index aada0bea2da9b..4710d3ae84ea3 100644 --- a/planning/planning_validator/src/utils.cpp +++ b/planning/planning_validator/src/utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/utils.hpp" +#include "autoware_planning_validator/utils.hpp" #include #include @@ -22,7 +22,7 @@ #include #include -namespace planning_validator +namespace autoware::planning_validator { using tier4_autoware_utils::calcCurvature; using tier4_autoware_utils::calcDistance2d; @@ -324,4 +324,4 @@ void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal) pose.position.y += std::sin(yaw) * longitudinal; } -} // namespace planning_validator +} // namespace autoware::planning_validator diff --git a/planning/planning_validator/test/src/test_planning_validator_functions.cpp b/planning/planning_validator/test/src/test_planning_validator_functions.cpp index e406b6c5d7da2..41ae41a80bdc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_functions.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_functions.cpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/debug_marker.hpp" -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/debug_marker.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -21,8 +21,8 @@ #include +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; -using planning_validator::PlanningValidator; TEST(PlanningValidatorTestSuite, checkValidFiniteValueFunction) { diff --git a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp index 2b2a32bf54618..78f57e0fe104d 100644 --- a/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_node_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include #include @@ -22,8 +22,8 @@ #include +using autoware::planning_validator::PlanningValidator; using planning_test_utils::PlanningInterfaceTestManager; -using planning_validator::PlanningValidator; std::shared_ptr generateTestManager() { @@ -46,7 +46,7 @@ std::shared_ptr generateNode() const auto planning_test_utils_dir = ament_index_cpp::get_package_share_directory("planning_test_utils"); const auto planning_validator_dir = - ament_index_cpp::get_package_share_directory("planning_validator"); + ament_index_cpp::get_package_share_directory("autoware_planning_validator"); node_options.arguments( {"--ros-args", "--params-file", planning_test_utils_dir + "/config/test_vehicle_info.param.yaml", "--params-file", diff --git a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp index 2329a3b9bb6c8..8cf56525edbc5 100644 --- a/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp +++ b/planning/planning_validator/test/src/test_planning_validator_pubsub.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "planning_validator/planning_validator.hpp" +#include "autoware_planning_validator/planning_validator.hpp" #include "test_parameter.hpp" #include "test_planning_validator_helper.hpp" @@ -29,11 +29,11 @@ * This test checks the diagnostics message published from the planning_validator node */ +using autoware::planning_validator::PlanningValidator; using autoware_planning_msgs::msg::Trajectory; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; using nav_msgs::msg::Odometry; -using planning_validator::PlanningValidator; constexpr double epsilon = 0.001; constexpr double scale_margin = 1.1; @@ -101,7 +101,8 @@ bool hasError(const std::vector & diags, const throw std::runtime_error(name + " is not contained in the diagnostic message."); } -std::pair, std::shared_ptr> +std::pair< + std::shared_ptr, std::shared_ptr> prepareTest(const Trajectory & trajectory, const Odometry & ego_odom) { auto validator = std::make_shared(getNodeOptionsWithDefaultParams()); diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index ded596a8a9898..b3be5be12b75d 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -4,27 +4,32 @@ project(imu_corrector) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_library(gyro_bias_estimation_module SHARED - src/gyro_bias_estimation_module.cpp -) - -ament_auto_add_executable(imu_corrector +ament_auto_add_library(${PROJECT_NAME} SHARED src/imu_corrector_core.cpp - src/imu_corrector_node.cpp ) -ament_auto_add_executable(gyro_bias_estimator +ament_auto_add_library(gyro_bias_estimator SHARED src/gyro_bias_estimator.cpp - src/gyro_bias_estimator_node.cpp + src/gyro_bias_estimation_module.cpp ) -target_link_libraries(gyro_bias_estimator gyro_bias_estimation_module) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "imu_corrector::ImuCorrector" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR SingleThreadedExecutor +) + +rclcpp_components_register_node(gyro_bias_estimator + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator_node + EXECUTOR SingleThreadedExecutor +) function(add_testcase filepath) get_filename_component(filename ${filepath} NAME) string(REGEX REPLACE ".cpp" "" test_name ${filename}) ament_add_gmock(${test_name} ${filepath}) - target_link_libraries("${test_name}" gyro_bias_estimation_module) + target_link_libraries("${test_name}" gyro_bias_estimator) ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) endfunction() diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml index e16ccef446aba..100168b17171a 100644 --- a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -6,7 +6,7 @@ - + diff --git a/sensing/imu_corrector/launch/imu_corrector.launch.xml b/sensing/imu_corrector/launch/imu_corrector.launch.xml index d87d6e77c110d..8430f2a50a85c 100755 --- a/sensing/imu_corrector/launch/imu_corrector.launch.xml +++ b/sensing/imu_corrector/launch/imu_corrector.launch.xml @@ -4,7 +4,7 @@ - + diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp index e99667ed1c4a6..d79626f66801b 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.cpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -23,8 +23,8 @@ namespace imu_corrector { -GyroBiasEstimator::GyroBiasEstimator() -: Node("gyro_bias_validator"), +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & options) +: rclcpp::Node("gyro_bias_validator", options), gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), @@ -244,3 +244,6 @@ void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusW } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp index 3592ff1f7d3b4..671c18de78f86 100644 --- a/sensing/imu_corrector/src/gyro_bias_estimator.hpp +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -42,7 +42,7 @@ class GyroBiasEstimator : public rclcpp::Node using Odometry = nav_msgs::msg::Odometry; public: - GyroBiasEstimator(); + explicit GyroBiasEstimator(const rclcpp::NodeOptions & options); private: void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp b/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp deleted file mode 100644 index a491957bbb57f..0000000000000 --- a/sensing/imu_corrector/src/gyro_bias_estimator_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 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 "gyro_bias_estimator.hpp" - -#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/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index a26c64925729c..c8e7698cb4e37 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -53,8 +53,9 @@ geometry_msgs::msg::Vector3 transformVector3( namespace imu_corrector { -ImuCorrector::ImuCorrector() -: Node("imu_corrector"), output_frame_(declare_parameter("base_link", "base_link")) +ImuCorrector::ImuCorrector(const rclcpp::NodeOptions & options) +: rclcpp::Node("imu_corrector", options), + output_frame_(declare_parameter("base_link", "base_link")) { transform_listener_ = std::make_shared(this); @@ -123,3 +124,6 @@ void ImuCorrector::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_m } } // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::ImuCorrector) diff --git a/sensing/imu_corrector/src/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp index 7709c611ab714..4d5377274ae78 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -34,7 +34,7 @@ class ImuCorrector : public rclcpp::Node using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX; public: - ImuCorrector(); + explicit ImuCorrector(const rclcpp::NodeOptions & options); private: void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr); diff --git a/sensing/imu_corrector/src/imu_corrector_node.cpp b/sensing/imu_corrector/src/imu_corrector_node.cpp deleted file mode 100644 index c1df5bee28439..0000000000000 --- a/sensing/imu_corrector/src/imu_corrector_node.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2023 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 "imu_corrector_core.hpp" - -#include - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -}