feat: apply autoware_ prefix for predicted_path_checker (#9985)

* feat(predicted_path_checker): apply `autoware_` prefix (see below):

  Note:
    * In this commit, I did not organize a folder structure.
      The folder structure will be organized in the next some commits.
    * The changes will follow the Autoware's guideline as below:
        - https://autowarefoundation.github.io/autoware-documentation/main/contributing/coding-guidelines/ros-nodes/directory-structure/#package-folder

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* rename(predicted_path_checker): move headers under `include/autoware`

  * Fixes due to this changes for .hpp/.cpp files will be applied in the next commit

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* fix(predicted_path_checker): fix include header paths

  * To follow the previous commit

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* rename: `predicted_path_checker` => `autoware_predicted_path_checker`

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug(autoware_predicted_path_checker): fix inconsistent namespacings

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* bug(autoware_predicted_path_checker): do not change node name

  * This might contaminate topic name

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

* style(pre-commit): autofix

* bug(tier4_control_launch): fix wrong package/plugin names

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>

---------

Signed-off-by: Junya Sasaki <junya.sasaki@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

control/predicted_path_checker/CMakeLists.txt → control/autoware_predicted_path_checker/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(predicted_path_checker)
+project(autoware_predicted_path_checker)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -12,17 +12,17 @@ include_directories(
   ${Eigen3_INCLUDE_DIRS}
 )
 
-ament_auto_add_library(predicted_path_checker SHARED
+ament_auto_add_library(${PROJECT_NAME} SHARED
   src/predicted_path_checker_node/predicted_path_checker_node.cpp
   src/predicted_path_checker_node/collision_checker.cpp
   src/predicted_path_checker_node/utils.cpp
   src/predicted_path_checker_node/debug_marker.cpp
 
 )
 
-rclcpp_components_register_node(predicted_path_checker
-  PLUGIN "autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode"
-  EXECUTABLE predicted_path_checker_node
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "autoware::predicted_path_checker::PredictedPathCheckerNode"
+  EXECUTABLE ${PROJECT_NAME}_node
 )
 
 if(BUILD_TESTING)

control/predicted_path_checker/include/predicted_path_checker/collision_checker.hpp → control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/collision_checker.hpp

@@ -12,17 +12,17 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
-#define PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
+#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
+#define AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
 
 #include <autoware/motion_utils/trajectory/conversion.hpp>
 #include <autoware/motion_utils/trajectory/interpolation.hpp>
+#include <autoware/predicted_path_checker/debug_marker.hpp>
+#include <autoware/predicted_path_checker/utils.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/debug_publisher.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
-#include <predicted_path_checker/debug_marker.hpp>
-#include <predicted_path_checker/utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -40,7 +40,7 @@
 #include <utility>
 #include <vector>
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -123,6 +123,6 @@ class CollisionChecker
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
   std::vector<PredictedObjectWithDetectionTime> predicted_object_history_{};
 };
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker
 
-#endif  // PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_
+#endif  // AUTOWARE__PREDICTED_PATH_CHECKER__COLLISION_CHECKER_HPP_

control/predicted_path_checker/include/predicted_path_checker/debug_marker.hpp → control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/debug_marker.hpp

@@ -11,8 +11,8 @@
 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 // See the License for the specific language governing permissions and
 // limitations under the License.
-#ifndef PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
-#define PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
+#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
+#define AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -35,7 +35,7 @@
 #include <eigen3/Eigen/Core>
 #include <eigen3/Eigen/Geometry>
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 
 enum class PolygonType : int8_t { Vehicle = 0, Collision };
@@ -87,6 +87,6 @@ class PredictedPathCheckerDebugNode
   std::vector<std::vector<Eigen::Vector3d>> collision_polyhedrons_;
 };
 
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker
 
-#endif  // PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_
+#endif  // AUTOWARE__PREDICTED_PATH_CHECKER__DEBUG_MARKER_HPP_

control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp → control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/predicted_path_checker_node.hpp

@@ -12,20 +12,20 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
-#define PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
+#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
+#define AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
 
 #include <autoware/component_interface_specs_universe/control.hpp>
 #include <autoware/component_interface_utils/rclcpp.hpp>
 #include <autoware/motion_utils/trajectory/conversion.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
+#include <autoware/predicted_path_checker/collision_checker.hpp>
+#include <autoware/predicted_path_checker/utils.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/self_pose_listener.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <diagnostic_updater/diagnostic_updater.hpp>
-#include <predicted_path_checker/collision_checker.hpp>
-#include <predicted_path_checker/utils.hpp>
 
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
@@ -45,7 +45,7 @@
 #include <utility>
 #include <vector>
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
@@ -178,6 +178,6 @@ class PredictedPathCheckerNode : public rclcpp::Node
   // Diagnostic Updater
   diagnostic_updater::Updater updater_;
 };
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker
 
-#endif  // PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_
+#endif  // AUTOWARE__PREDICTED_PATH_CHECKER__PREDICTED_PATH_CHECKER_NODE_HPP_

control/predicted_path_checker/include/predicted_path_checker/utils.hpp → control/autoware_predicted_path_checker/include/autoware/predicted_path_checker/utils.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef PREDICTED_PATH_CHECKER__UTILS_HPP_
-#define PREDICTED_PATH_CHECKER__UTILS_HPP_
+#ifndef AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_
+#define AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_
 
 #include <autoware/interpolation/linear_interpolation.hpp>
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
@@ -36,7 +36,7 @@
 #include <utility>
 #include <vector>
 
-namespace utils
+namespace autoware::predicted_path_checker
 {
 
 using autoware::universe_utils::Point2d;
@@ -99,6 +99,6 @@ void getCurrentObjectPose(
   const rclcpp::Time & current_time);
 
 bool isFrontObstacle(const Pose & ego_pose, const geometry_msgs::msg::Point & obstacle_pos);
-}  // namespace utils
+}  // namespace autoware::predicted_path_checker
 
-#endif  // PREDICTED_PATH_CHECKER__UTILS_HPP_
+#endif  // AUTOWARE__PREDICTED_PATH_CHECKER__UTILS_HPP_

control/predicted_path_checker/launch/predicted_path_checker.launch.xml → control/autoware_predicted_path_checker/launch/predicted_path_checker.launch.xml

@@ -5,11 +5,11 @@
   <arg name="input/odometry" default="/localization/kinematic_state"/>
   <arg name="input/current_accel" default="/localization/acceleration"/>
 
-  <arg name="config_file" default="$(find-pkg-share predicted_path_checker)/config/predicted_path_checker.param.yaml"/>
+  <arg name="config_file" default="$(find-pkg-share autoware_predicted_path_checker)/config/predicted_path_checker.param.yaml"/>
   <!-- vehicle info -->
   <arg name="vehicle_info_param_file" default="$(find-pkg-share autoware_vehicle_info_utils)/config/vehicle_info.param.yaml"/>
 
-  <node pkg="predicted_path_checker" exec="predicted_path_checker_node" name="predicted_path_checker_node" output="screen">
+  <node pkg="autoware_predicted_path_checker" exec="autoware_predicted_path_checker_node" name="predicted_path_checker_node" output="screen">
     <param from="$(var config_file)"/>
     <param from="$(var vehicle_info_param_file)"/>
     <remap from="~/input/objects" to="$(var input/objects)"/>

control/predicted_path_checker/package.xml → control/autoware_predicted_path_checker/package.xml

@@ -1,11 +1,12 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>predicted_path_checker</name>
+  <name>autoware_predicted_path_checker</name>
   <version>0.40.0</version>
   <description>The predicted_path_checker package</description>
 
   <maintainer email="berkay@leodrive.ai">Berkay Karaman</maintainer>
+  <maintainer email="junya.sasaki@tier4.jp">Junya Sasaki</maintainer>
   <license>Apache 2.0</license>
   <author email="berkay@leodrive.ai">Berkay Karaman</author>
 

control/predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp → control/autoware_predicted_path_checker/src/predicted_path_checker_node/collision_checker.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "predicted_path_checker/collision_checker.hpp"
+#include "autoware/predicted_path_checker/collision_checker.hpp"
 
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 #include <rclcpp/logging.hpp>
@@ -22,7 +22,7 @@
 #include <utility>
 #include <vector>
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 CollisionChecker::CollisionChecker(
   rclcpp::Node * node, std::shared_ptr<PredictedPathCheckerDebugNode> debug_ptr)
@@ -61,7 +61,7 @@ CollisionChecker::checkTrajectoryForCollision(
 
     // create one-step polygon for vehicle
     Polygon2d one_step_move_vehicle_polygon2d =
-      utils::createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin);
+      createOneStepPolygon(p_front, p_back, vehicle_info_, param_.width_margin);
     if (param_.enable_z_axis_obstacle_filtering) {
       debug_ptr_->pushPolyhedron(
         one_step_move_vehicle_polygon2d, z_min, z_max, PolygonType::Vehicle);
@@ -126,7 +126,7 @@ CollisionChecker::checkObstacleHistory(
   std::vector<std::pair<geometry_msgs::msg::Point, PredictedObject>> collision_points_in_history;
   for (const auto & obj_history : predicted_object_history_) {
     if (param_.enable_z_axis_obstacle_filtering) {
-      if (!utils::intersectsInZAxis(obj_history.object, z_min, z_max)) {
+      if (!intersectsInZAxis(obj_history.object, z_min, z_max)) {
         continue;
       }
     }
@@ -169,11 +169,11 @@ CollisionChecker::checkDynamicObjects(
   for (size_t i = 0; i < dynamic_objects->objects.size(); ++i) {
     const auto & obj = dynamic_objects->objects.at(i);
     if (param_.enable_z_axis_obstacle_filtering) {
-      if (!utils::intersectsInZAxis(obj, z_min, z_max)) {
+      if (!intersectsInZAxis(obj, z_min, z_max)) {
         continue;
       }
     }
-    const auto object_polygon = utils::convertObjToPolygon(obj);
+    const auto object_polygon = convertObjToPolygon(obj);
     if (object_polygon.outer().empty()) {
       // unsupported type
       continue;
@@ -205,7 +205,7 @@ CollisionChecker::checkDynamicObjects(
       }
       geometry_msgs::msg::Point nearest_collision_point_tmp;
 
-      double norm = utils::getNearestPointAndDistanceForPredictedObject(
+      double norm = getNearestPointAndDistanceForPredictedObject(
         collision_point_array, base_pose, &nearest_collision_point_tmp);
       if (norm < min_norm_collision_norm || !is_init) {
         min_norm_collision_norm = norm;
@@ -217,7 +217,7 @@ CollisionChecker::checkDynamicObjects(
   }
   if (is_init) {
     const auto & obj = dynamic_objects->objects.at(nearest_collision_object_index);
-    const auto obstacle_polygon = utils::convertObjToPolygon(obj);
+    const auto obstacle_polygon = convertObjToPolygon(obj);
     if (param_.enable_z_axis_obstacle_filtering) {
       debug_ptr_->pushPolyhedron(obstacle_polygon, z_min, z_max, PolygonType::Collision);
     } else {
@@ -229,4 +229,4 @@ CollisionChecker::checkDynamicObjects(
   }
   return boost::none;
 }
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker

control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp → control/autoware_predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "predicted_path_checker/debug_marker.hpp"
+#include "autoware/predicted_path_checker/debug_marker.hpp"
 
 #include <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
@@ -38,7 +38,7 @@ using autoware::universe_utils::createMarkerOrientation;
 using autoware::universe_utils::createMarkerScale;
 using autoware::universe_utils::createPoint;
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode(
   rclcpp::Node * node, const double base_link2front)
@@ -326,4 +326,4 @@ visualization_msgs::msg::MarkerArray PredictedPathCheckerDebugNode::makeVisualiz
   return msg;
 }
 
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker

control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp → control/autoware_predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "predicted_path_checker/predicted_path_checker_node.hpp"
+#include "autoware/predicted_path_checker/predicted_path_checker_node.hpp"
 
 #include <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/motion_utils/resample/resample.hpp>
@@ -26,7 +26,7 @@
 #include <utility>
 #include <vector>
 
-namespace autoware::motion::control::predicted_path_checker
+namespace autoware::predicted_path_checker
 {
 
 PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & node_options)
@@ -301,7 +301,7 @@ void PredictedPathCheckerNode::onTimer()
 
     // Check if the vehicle is in the brake distance
 
-    const bool is_in_brake_distance = utils::isInBrakeDistance(
+    const bool is_in_brake_distance = isInBrakeDistance(
       predicted_trajectory_array, stop_idx, relative_velocity, relative_acceleration,
       node_param_.max_deceleration, node_param_.delay_time);
 
@@ -431,7 +431,7 @@ bool PredictedPathCheckerNode::isItDiscretePoint(
   const auto nearest_segment =
     autoware::motion_utils::findNearestSegmentIndex(reference_trajectory, collision_point.pose);
 
-  const auto nearest_point = utils::calcInterpolatedPoint(
+  const auto nearest_point = calcInterpolatedPoint(
     reference_trajectory, collision_point.pose.position, *nearest_segment, false);
 
   const auto distance = autoware::universe_utils::calcDistance2d(
@@ -495,7 +495,7 @@ void PredictedPathCheckerNode::extendTrajectoryPointsArray(TrajectoryPoints & tr
   // collision_point.
   const double extend_distance = vehicle_info_.max_longitudinal_offset_m + node_param_.stop_margin;
   const auto & goal_point = trajectory.back();
-  const auto trajectory_point_extend = utils::getExtendTrajectoryPoint(extend_distance, goal_point);
+  const auto trajectory_point_extend = getExtendTrajectoryPoint(extend_distance, goal_point);
   trajectory.push_back(trajectory_point_extend);
 }
 
@@ -506,14 +506,14 @@ size_t PredictedPathCheckerNode::insertStopPoint(
     autoware::motion_utils::findNearestSegmentIndex(trajectory, collision_point);
 
   const auto nearest_collision_point =
-    utils::calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true);
+    calcInterpolatedPoint(trajectory, collision_point, nearest_collision_segment, true);
 
   const size_t collision_idx = nearest_collision_segment + 1;
 
   trajectory.insert(trajectory.begin() + static_cast<int>(collision_idx), nearest_collision_point);
 
   const auto stop_point =
-    utils::findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_);
+    findStopPoint(trajectory, collision_idx, node_param_.stop_margin, vehicle_info_);
 
   const size_t stop_idx = stop_point.first + 1;
   trajectory.insert(trajectory.begin() + static_cast<int>(stop_idx), stop_point.second);
@@ -550,13 +550,12 @@ void PredictedPathCheckerNode::filterObstacles(
 
   for (auto & object : object_ptr_.get()->objects) {
     // Check is it in front of ego vehicle
-    if (!utils::isFrontObstacle(
-          ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {
+    if (!isFrontObstacle(ego_pose, object.kinematics.initial_pose_with_covariance.pose.position)) {
       continue;
     }
 
     // Check is it near to trajectory
-    const double max_length = utils::calcObstacleMaxLength(object.shape);
+    const double max_length = calcObstacleMaxLength(object.shape);
     const size_t seg_idx = autoware::motion_utils::findNearestSegmentIndex(
       traj, object.kinematics.initial_pose_with_covariance.pose.position);
     const auto p_front = autoware::universe_utils::getPoint(traj.at(seg_idx));
@@ -580,14 +579,13 @@ void PredictedPathCheckerNode::filterObstacles(
     }
     PredictedObject filtered_object = object;
     if (use_prediction) {
-      utils::getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now());
+      getCurrentObjectPose(filtered_object, object_ptr_.get()->header.stamp, this->now());
     }
     filtered_objects.objects.push_back(filtered_object);
   }
 }
 
-}  // namespace autoware::motion::control::predicted_path_checker
+}  // namespace autoware::predicted_path_checker
 
 #include <rclcpp_components/register_node_macro.hpp>
-RCLCPP_COMPONENTS_REGISTER_NODE(
-  autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(autoware::predicted_path_checker::PredictedPathCheckerNode)