diff --git a/planning/autoware_planning_factor_interface/CMakeLists.txt b/planning/autoware_planning_factor_interface/CMakeLists.txt new file mode 100644 index 000000000..0c25a2c21 --- /dev/null +++ b/planning/autoware_planning_factor_interface/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_planning_factor_interface) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(autoware_planning_factor_interface SHARED + DIRECTORY src +) + +ament_auto_package() diff --git a/planning/autoware_planning_factor_interface/README.md b/planning/autoware_planning_factor_interface/README.md new file mode 100644 index 000000000..9a037f8f4 --- /dev/null +++ b/planning/autoware_planning_factor_interface/README.md @@ -0,0 +1,66 @@ +# autoware_planning_factor_interface + +## Overview + +The `PlanningFactorInterface` is a C++ class designed to facilitate the addition and publication of planning factors. + +## Design + +The `PlanningFactorInterface` class is designed to be lightweight and efficient, with the following key components: + +- **Add:** Methods to add planning factors to the interface. + +- **Publisher:** The class includes a publisher for `PlanningFactorArray` messages, which are used to distribute planning factors to other nodes in the system. + +The design emphasizes flexibility and ease of use, allowing developers to quickly integrate new planning factors into autoware. + +## Usage + +### Including the Header + +To use the `PlanningFactorInterface`, include the header file in your code: + +```cpp +#include +``` + +### Creating an Instance + +Instantiate the `PlanningFactorInterface` by providing a node and a name for the factor module: + +```cpp + +class PlannerInterface +{ +public: + virtual ~PlannerInterface() = default; + PlannerInterface( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) + : planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( + &node, "obstacle_cruise_planner")}, +``` + +code example from src/universe/autoware.universe/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp + +### Adding Planning Factors + +The `add` method can be used to add planning factors. Here's an example from src/universe/autoware.universe/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp. + +```cpp +planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + autoware_internal_planning_msgs::msg::PlanningFactor::NONE, + autoware_internal_planning_msgs::msg::SafetyFactorArray{}); +``` + +### Publishing Factors + +After adding planning factors, you can publish them by calling the `publish` method: + +```cpp +// Publish the added factors +planning_factor_interface_->publish(); +``` diff --git a/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp new file mode 100644 index 000000000..a88fdbb21 --- /dev/null +++ b/planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp @@ -0,0 +1,242 @@ +// Copyright 2024 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. + +#ifndef AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ +#define AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace autoware::planning_factor_interface +{ + +using autoware_internal_planning_msgs::msg::ControlPoint; +using autoware_internal_planning_msgs::msg::PlanningFactor; +using autoware_internal_planning_msgs::msg::PlanningFactorArray; +using autoware_internal_planning_msgs::msg::SafetyFactorArray; +using geometry_msgs::msg::Pose; + +class PlanningFactorInterface +{ +public: + PlanningFactorInterface(rclcpp::Node * node, const std::string & name) + : name_{name}, + pub_factors_{ + node->create_publisher("/planning/planning_factors/" + name, 1)}, + clock_{node->get_clock()} + { + } + + /** + * @brief factor setter for single control point. + * + * @param path points. + * @param ego current pose. + * @param control point pose. (e.g. stop or slow down point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & control_point_pose, + const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto distance = static_cast(autoware::motion_utils::calcSignedArcLength( + points, ego_pose.position, control_point_pose.position)); + add( + distance, control_point_pose, behavior, safety_factors, is_driving_forward, velocity, + shift_length, detail); + } + + /** + * @brief factor setter for two control points (section). + * + * @param path points. + * @param ego current pose. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + template + void add( + const std::vector & points, const Pose & ego_pose, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto start_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, start_pose.position)); + const auto end_distance = static_cast( + autoware::motion_utils::calcSignedArcLength(points, ego_pose.position, end_pose.position)); + add( + start_distance, end_distance, start_pose, end_pose, behavior, safety_factors, + is_driving_forward, velocity, shift_length, detail); + } + + /** + * @brief factor setter for single control point. + * + * @param distance to control point. + * @param control point pose. (e.g. stop point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double distance, const Pose & control_point_pose, const uint16_t behavior, + const SafetyFactorArray & safety_factors, const bool is_driving_forward = true, + const double velocity = 0.0, const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_point = autoware_internal_planning_msgs::build() + .pose(control_point_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(distance); + + const auto factor = autoware_internal_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief factor setter for two control points (section). + * + * @param distance to control section start point. + * @param distance to control section end point. + * @param control section start pose. (e.g. lane change start point pose) + * @param control section end pose. (e.g. lane change end point pose) + * @param behavior of this planning factor. + * @param safety factor. + * @param driving direction. + * @param target velocity of the control point. + * @param shift length of the control point. + * @param detail information. + */ + void add( + const double start_distance, const double end_distance, const Pose & start_pose, + const Pose & end_pose, const uint16_t behavior, const SafetyFactorArray & safety_factors, + const bool is_driving_forward = true, const double velocity = 0.0, + const double shift_length = 0.0, const std::string & detail = "") + { + const auto control_start_point = autoware_internal_planning_msgs::build() + .pose(start_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(start_distance); + + const auto control_end_point = autoware_internal_planning_msgs::build() + .pose(end_pose) + .velocity(velocity) + .shift_length(shift_length) + .distance(end_distance); + + const auto factor = autoware_internal_planning_msgs::build() + .module(name_) + .is_driving_forward(is_driving_forward) + .control_points({control_start_point, control_end_point}) + .behavior(behavior) + .detail(detail) + .safety_factors(safety_factors); + + factors_.push_back(factor); + } + + /** + * @brief publish planning factors. + */ + void publish() + { + PlanningFactorArray msg; + msg.header.frame_id = "map"; + msg.header.stamp = clock_->now(); + msg.factors = factors_; + + pub_factors_->publish(msg); + + factors_.clear(); + } + +private: + std::string name_; + + rclcpp::Publisher::SharedPtr pub_factors_; + + rclcpp::Clock::SharedPtr clock_; + + std::vector factors_; +}; + +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +extern template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +extern template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); + +} // namespace autoware::planning_factor_interface + +#endif // AUTOWARE__PLANNING_FACTOR_INTERFACE__PLANNING_FACTOR_INTERFACE_HPP_ diff --git a/planning/autoware_planning_factor_interface/package.xml b/planning/autoware_planning_factor_interface/package.xml new file mode 100644 index 000000000..c3b388547 --- /dev/null +++ b/planning/autoware_planning_factor_interface/package.xml @@ -0,0 +1,29 @@ + + + + autoware_planning_factor_interface + 0.0.0 + The autoware_planning_factor_interface package + Satoshi Ota + Mamoru Sobue + Apache License 2.0 + + Satoshi Ota + + ament_cmake_auto + autoware_cmake + + autoware_internal_planning_msgs + autoware_motion_utils + autoware_planning_msgs + autoware_utils + rclcpp + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp new file mode 100644 index 000000000..de9a8b760 --- /dev/null +++ b/planning/autoware_planning_factor_interface/src/planing_factor_interface.cpp @@ -0,0 +1,49 @@ +// Copyright 2024 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 + +#include +#include + +namespace autoware::planning_factor_interface +{ +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const uint16_t behavior, const SafetyFactorArray &, const bool, const double, const double, + const std::string &); + +template void +PlanningFactorInterface::add( + const std::vector &, const Pose &, + const Pose &, const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, + const double, const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +template void PlanningFactorInterface::add( + const std::vector &, const Pose &, const Pose &, + const Pose &, const uint16_t behavior, const SafetyFactorArray &, const bool, const double, + const double, const std::string &); +} // namespace autoware::planning_factor_interface