refactor obstacle_cruise_planner

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp

@@ -15,25 +15,16 @@
 #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_
 #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_
 
+#include "obstacle_cruise_planner/type_alias.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "visualization_msgs/msg/marker_array.hpp"
-
 #include <boost/optional.hpp>
 
 #include <string>
 #include <vector>
 
-using autoware_auto_perception_msgs::msg::ObjectClassification;
-using autoware_auto_perception_msgs::msg::PredictedObject;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_perception_msgs::msg::Shape;
-
 namespace
 {
 std::string toHexString(const unique_identifier_msgs::msg::UUID & id)
@@ -85,7 +76,7 @@ struct TargetObstacle
 struct ObstacleCruisePlannerData
 {
   rclcpp::Time current_time;
-  autoware_auto_planning_msgs::msg::Trajectory traj;
+  Trajectory traj;
   geometry_msgs::msg::Pose current_pose;
   double current_vel;
   double current_acc;
@@ -115,8 +106,8 @@ struct DebugData
   std::vector<PredictedObject> intentionally_ignored_obstacles;
   std::vector<TargetObstacle> obstacles_to_stop;
   std::vector<TargetObstacle> obstacles_to_cruise;
-  visualization_msgs::msg::MarkerArray stop_wall_marker;
-  visualization_msgs::msg::MarkerArray cruise_wall_marker;
+  MarkerArray stop_wall_marker;
+  MarkerArray cruise_wall_marker;
   std::vector<tier4_autoware_utils::Polygon2d> detection_polygons;
   std::vector<geometry_msgs::msg::Point> collision_points;
 };

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -18,47 +18,19 @@
 #include "obstacle_cruise_planner/common_structs.hpp"
 #include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp"
 #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp"
+#include "obstacle_cruise_planner/type_alias.hpp"
 #include "signal_processing/lowpass_filter_1d.hpp"
 #include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
-#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "geometry_msgs/msg/accel_stamped.hpp"
-#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp"
-#include "geometry_msgs/msg/point_stamped.hpp"
-#include "geometry_msgs/msg/transform_stamped.hpp"
-#include "geometry_msgs/msg/twist_stamped.hpp"
-#include "nav_msgs/msg/odometry.hpp"
-#include "tier4_debug_msgs/msg/float32_stamped.hpp"
-#include "tier4_planning_msgs/msg/velocity_limit.hpp"
-#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp"
-#include "visualization_msgs/msg/marker_array.hpp"
-
 #include <boost/optional.hpp>
 
 #include <memory>
 #include <mutex>
 #include <string>
 #include <vector>
 
-using autoware_auto_perception_msgs::msg::ObjectClassification;
-using autoware_auto_perception_msgs::msg::PredictedObject;
-using autoware_auto_perception_msgs::msg::PredictedObjects;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using geometry_msgs::msg::AccelStamped;
-using geometry_msgs::msg::AccelWithCovarianceStamped;
-using nav_msgs::msg::Odometry;
-using tier4_debug_msgs::msg::Float32Stamped;
-using tier4_planning_msgs::msg::StopReasonArray;
-using tier4_planning_msgs::msg::VelocityLimit;
-using tier4_planning_msgs::msg::VelocityLimitClearCommand;
-using vehicle_info_util::VehicleInfo;
-
 namespace motion_planning
 {
 class ObstacleCruisePlannerNode : public rclcpp::Node
@@ -70,9 +42,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   // callback functions
   rcl_interfaces::msg::SetParametersResult onParam(
     const std::vector<rclcpp::Parameter> & parameters);
-  void onObjects(const PredictedObjects::ConstSharedPtr msg);
-  void onOdometry(const Odometry::ConstSharedPtr);
-  void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr);
   void onTrajectory(const Trajectory::ConstSharedPtr msg);
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
@@ -126,9 +95,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Publisher<Trajectory>::SharedPtr trajectory_pub_;
   rclcpp::Publisher<VelocityLimit>::SharedPtr vel_limit_pub_;
   rclcpp::Publisher<VelocityLimitClearCommand>::SharedPtr clear_vel_limit_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_cruise_wall_marker_pub_;
-  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_stop_wall_marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_cruise_wall_marker_pub_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr debug_stop_wall_marker_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_stop_planning_info_pub_;
   rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr debug_cruise_planning_info_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;
@@ -145,8 +114,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   // data for callback functions
   PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr};
-  geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr};
-  geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr};
+  Odometry::ConstSharedPtr current_odom_ptr_{nullptr};
+  AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_{nullptr};
 
   // Vehicle Parameters
   VehicleInfo vehicle_info_;

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp

@@ -18,27 +18,20 @@
 #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp"
 #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp"
 #include "obstacle_cruise_planner/planner_interface.hpp"
+#include "obstacle_cruise_planner/type_alias.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/utilities.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include <tier4_debug_msgs/msg/float32_stamped.hpp>
-#include <visualization_msgs/msg/marker_array.hpp>
-
 #include <boost/optional.hpp>
 
 #include <memory>
 #include <tuple>
 #include <vector>
 
-using autoware_auto_perception_msgs::msg::ObjectClassification;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using tier4_debug_msgs::msg::Float32Stamped;
-
 class OptimizationBasedPlanner : public PlannerInterface
 {
 public:
@@ -77,8 +70,7 @@ class OptimizationBasedPlanner : public PlannerInterface
     const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::PointStamped & point);
 
   boost::optional<double> calcTrajectoryLengthFromCurrentPose(
-    const autoware_auto_planning_msgs::msg::Trajectory & traj,
-    const geometry_msgs::msg::Pose & current_pose);
+    const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose);
 
   geometry_msgs::msg::Pose transformBaseLink2Center(
     const geometry_msgs::msg::Pose & pose_base_link);

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -18,31 +18,15 @@
 #include "motion_utils/motion_utils.hpp"
 #include "obstacle_cruise_planner/common_structs.hpp"
 #include "obstacle_cruise_planner/stop_planning_debug_info.hpp"
+#include "obstacle_cruise_planner/type_alias.hpp"
 #include "obstacle_cruise_planner/utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
-#include "vehicle_info_util/vehicle_info_util.hpp"
-
-#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "tier4_planning_msgs/msg/stop_reason_array.hpp"
-#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
-#include "tier4_planning_msgs/msg/velocity_limit.hpp"
-#include "visualization_msgs/msg/marker_array.hpp"
 
 #include <boost/optional.hpp>
 
 #include <memory>
 #include <vector>
 
-using autoware_adapi_v1_msgs::msg::VelocityFactor;
-using autoware_adapi_v1_msgs::msg::VelocityFactorArray;
-using autoware_auto_perception_msgs::msg::ObjectClassification;
-using autoware_auto_planning_msgs::msg::Trajectory;
-using autoware_auto_planning_msgs::msg::TrajectoryPoint;
-using tier4_debug_msgs::msg::Float32MultiArrayStamped;
-using tier4_planning_msgs::msg::StopSpeedExceeded;
-using tier4_planning_msgs::msg::VelocityLimit;
-
 class PlannerInterface
 {
 public:
@@ -53,8 +37,7 @@ class PlannerInterface
     vehicle_info_(vehicle_info),
     ego_nearest_param_(ego_nearest_param)
   {
-    stop_reasons_pub_ =
-      node.create_publisher<tier4_planning_msgs::msg::StopReasonArray>("~/output/stop_reasons", 1);
+    stop_reasons_pub_ = node.create_publisher<StopReasonArray>("~/output/stop_reasons", 1);
     velocity_factors_pub_ =
       node.create_publisher<VelocityFactorArray>("/planning/velocity_factors/obstacle_cruise", 1);
     stop_speed_exceeded_pub_ =
@@ -126,7 +109,7 @@ class PlannerInterface
   double nearest_yaw_deviation_threshold_;
 
   // Publishers
-  rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reasons_pub_;
+  rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reasons_pub_;
   rclcpp::Publisher<VelocityFactorArray>::SharedPtr velocity_factors_pub_;
   rclcpp::Publisher<StopSpeedExceeded>::SharedPtr stop_speed_exceeded_pub_;
 

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp

@@ -15,14 +15,10 @@
 #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_
 #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_
 
+#include "obstacle_cruise_planner/type_alias.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
-#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
-#include "autoware_auto_planning_msgs/msg/trajectory.hpp"
-#include "geometry_msgs/msg/point32.hpp"
-#include "geometry_msgs/msg/pose.hpp"
-
 #include <boost/geometry.hpp>
 #include <boost/optional.hpp>
 
@@ -36,39 +32,36 @@ using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 
 boost::optional<size_t> getCollisionIndex(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const std::vector<Polygon2d> & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose,
-  const autoware_auto_perception_msgs::msg::Shape & shape,
+  const Trajectory & traj, const std::vector<Polygon2d> & traj_polygons,
+  const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape,
   std::vector<geometry_msgs::msg::PointStamped> & collision_points,
   const double max_dist = std::numeric_limits<double>::max());
 
 std::vector<geometry_msgs::msg::PointStamped> getCollisionPoints(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const std::vector<Polygon2d> & traj_polygons, const std_msgs::msg::Header & obj_header,
-  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
-  const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
+  const Trajectory & traj, const std::vector<Polygon2d> & traj_polygons,
+  const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path,
+  const Shape & shape, const rclcpp::Time & current_time,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward,
   std::vector<size_t> & collision_index, const double max_dist = std::numeric_limits<double>::max(),
   const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());
 
 std::vector<geometry_msgs::msg::PointStamped> willCollideWithSurroundObstacle(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const std::vector<Polygon2d> & traj_polygons, const std_msgs::msg::Header & obj_header,
-  const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
-  const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time,
-  const double max_dist, const double ego_obstacle_overlap_time_threshold,
+  const Trajectory & traj, const std::vector<Polygon2d> & traj_polygons,
+  const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path,
+  const Shape & shape, const rclcpp::Time & current_time, const double max_dist,
+  const double ego_obstacle_overlap_time_threshold,
   const double max_prediction_time_for_collision_check, std::vector<size_t> & collision_index,
   const double vehicle_max_longitudinal_offset, const bool is_driving_forward);
 
 std::vector<Polygon2d> createOneStepPolygons(
-  const autoware_auto_planning_msgs::msg::Trajectory & traj,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width);
+  const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info,
+  const double expand_width);
 
 geometry_msgs::msg::PointStamped calcNearestCollisionPoint(
   const size_t & first_within_idx,
   const std::vector<geometry_msgs::msg::PointStamped> & collision_points,
-  const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj,
-  const double vehicle_max_longitudinal_offset, const bool is_driving_forward);
+  const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset,
+  const bool is_driving_forward);
 }  // namespace polygon_utils
 
 #endif  // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_