Redesign

.github/workflows/build_and_test.yml

@@ -7,6 +7,9 @@ on:
       - main
   workflow_dispatch:
 
+env:
+  BUILD_DEPEND_FILE: https://raw.githubusercontent.com/tier4/autoware.proj/main/autoware.proj.repos
+
 jobs:
   build-and-test:
     runs-on: ubuntu-latest
@@ -22,13 +25,13 @@ jobs:
         with:
           required-ros-distributions: foxy
 
-      - name: Concat build_depends.repos
+      - name: Create build_depends.repos
         run: |
-          curl -sSL -H "Authorization: token ${{ secrets.REPO_TOKEN }}" https://raw.githubusercontent.com/tier4/autoware.iv/main/build_depends.repos | sed '1d' >> build_depends.repos
+          curl -sSL -H "Authorization: token ${{ secrets.REPO_TOKEN }}" ${{ env.BUILD_DEPEND_FILE }} | sed -e "s|[email protected]:|https://github.com/|g" > build_depends.repos
 
       - name: Run action-ros-ci
         id: action_ros_ci_step
-        uses: ros-tooling/action-ros-ci@add-private-repo-test
+        uses: ros-tooling/action-ros-ci@master
         with:
           package-name: obstacle_stop_planner_refine
           target-ros2-distro: foxy

.github/workflows/code_metrics_main.yml

@@ -47,7 +47,7 @@ jobs:
       - name: Create index page
         run: |
           cd pr-check
-          tree -H "$GITHUB_PAGES_URL/pr-check" \
+          tree -H "${{ env.GITHUB_PAGES_URL }}/pr-check" \
             -L 2 --noreport --charset utf-8 -T 'Index of code metrics' \
             -P "*.html" > index.html
 

obstacle_stop_planner_refine/README.md

@@ -190,7 +190,6 @@ Output:
 | `expand_slow_down_range` | double | offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] |
 | `max_slow_down_vel`      | double | max slow down velocity [m/s]                                                                   |
 | `min_slow_down_vel`      | double | min slow down velocity [m/s]                                                                   |
-| `max_deceleration`       | double | max slow down deceleration [m/s2]                                                              |
 
 ### adaptive_cruise_planner
 

obstacle_stop_planner_refine/config/obstacle_stop_planner.param.yaml

@@ -12,4 +12,3 @@
       expand_slow_down_range: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m]
       max_slow_down_vel: 1.38 # max slow down velocity [m/s]
       min_slow_down_vel: 0.28 # min slow down velocity [m/s]
-      max_deceleration: 2.0 # max slow down deceleration [m/s2]

obstacle_stop_planner_refine/include/obstacle_stop_planner/adaptive_cruise_control.hpp

@@ -17,6 +17,7 @@
 
 #include <vector>
 #include <tuple>
+#include <memory>
 
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -28,7 +29,7 @@
 #include "autoware_planning_msgs/msg/trajectory.hpp"
 #include "autoware_utils/autoware_utils.hpp"
 #include "obstacle_stop_planner/parameter/adaptive_cruise_control_parameter.hpp"
-#include "vehicle_info_util/vehicle_info.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
 
 namespace obstacle_stop_planner
 {
@@ -38,32 +39,47 @@ using autoware_utils::Polygon2d;
 using autoware_utils::Polygon3d;
 using boost::optional;
 
+namespace adaptive_cruise_controller
+{
+struct Input
+{
+  autoware_planning_msgs::msg::Trajectory trajectory;
+  size_t nearest_collision_point_idx;
+  geometry_msgs::msg::Pose self_pose;
+  Point2d nearest_collision_point;
+  rclcpp::Time nearest_collision_point_time;
+  autoware_perception_msgs::msg::DynamicObjectArray object_array;
+  geometry_msgs::msg::TwistStamped current_velocity;
+};
+}  // namespace adaptive_cruise_controller
+
 class AdaptiveCruiseController
 {
 public:
   AdaptiveCruiseController(
-    rclcpp::Node * node,
-    const vehicle_info_util::VehicleInfo & vehicle_info,
-    const AdaptiveCruiseControlParameter & acc_param);
-
-  std::tuple<bool, autoware_planning_msgs::msg::Trajectory> insertAdaptiveCruiseVelocity(
-    const autoware_planning_msgs::msg::Trajectory & trajectory,
-    const int nearest_collision_point_idx,
-    const geometry_msgs::msg::Pose self_pose, const Point2d & nearest_collision_point,
-    const rclcpp::Time & nearest_collision_point_time,
-    const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr,
-    const geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr,
-    const autoware_planning_msgs::msg::Trajectory & input_trajectory);
+    const rclcpp::node_interfaces::NodeLoggingInterface::ConstSharedPtr node_logging,
+    const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+    const std::shared_ptr<vehicle_info_util::VehicleInfo> & vehicle_info,
+    const std::shared_ptr<AdaptiveCruiseControlParameter> & acc_param);
+
+  void updateParameter(const std::shared_ptr<AdaptiveCruiseControlParameter> & acc_param);
+
+  boost::optional<autoware_planning_msgs::msg::Trajectory> insertAdaptiveCruiseVelocity(
+    const adaptive_cruise_controller::Input & input);
+
+  autoware_debug_msgs::msg::Float32MultiArrayStamped getDebugMsg() {return debug_values_;}
 
 private:
-  rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
+  // rclcpp::Publisher<autoware_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr pub_debug_;
+
+  rclcpp::node_interfaces::NodeLoggingInterface::ConstSharedPtr node_logging_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
 
-  rclcpp::Node * node_;
   /*
    * Parameter
    */
-  vehicle_info_util::VehicleInfo vehicle_info_;
-  AdaptiveCruiseControlParameter param_;
+  std::shared_ptr<vehicle_info_util::VehicleInfo> vehicle_info_;
+  std::shared_ptr<AdaptiveCruiseControlParameter> param_;
 
   rclcpp::Time prev_collision_point_time_;
   Point2d prev_collision_point_;
@@ -86,7 +102,7 @@ class AdaptiveCruiseController
   static double calcTrajYaw(
     const autoware_planning_msgs::msg::Trajectory & trajectory, const int collision_point_idx);
   optional<double> estimatePointVelocityFromObject(
-    const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr,
+    const autoware_perception_msgs::msg::DynamicObjectArray & object_array,
     const double traj_yaw,
     const Point2d & nearest_collision_point);
   optional<double> estimatePointVelocityFromPcl(
@@ -111,7 +127,7 @@ class AdaptiveCruiseController
   void registerQueToVelocity(const double vel, const rclcpp::Time & vel_time);
 
   /* Debug */
-  mutable autoware_debug_msgs::msg::Float32MultiArrayStamped debug_values_;
+  autoware_debug_msgs::msg::Float32MultiArrayStamped debug_values_;
   enum DBGVAL
   {
     ESTIMATED_VEL_PCL = 0,

obstacle_stop_planner_refine/include/obstacle_stop_planner/debug_marker.hpp

@@ -34,23 +34,26 @@ enum class PoseType : int8_t { Stop = 0, SlowDownStart, SlowDownEnd };
 class ObstacleStopPlannerDebugNode
 {
 public:
-  explicit ObstacleStopPlannerDebugNode(rclcpp::Node * node, const double base_link2front);
+  explicit ObstacleStopPlannerDebugNode(
+    const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock,
+    const double base_link2front);
   ~ObstacleStopPlannerDebugNode() {}
   bool pushPolygon(
     const Polygon2d & polygon, const double z, const PolygonType & type);
+  void pushPolygons(
+    const std::vector<Polygon2d> & polygons, const std::vector<double> & z,
+    const PolygonType & type);
   bool pushPolygon(const Polygon3d & polygon, const PolygonType & type);
   bool pushPose(const geometry_msgs::msg::Pose & pose, const PoseType & type);
   bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type);
   bool pushObstaclePoint(const Point3d & obstacle_point, const PointType & type);
   visualization_msgs::msg::MarkerArray makeVisualizationMarker();
   autoware_planning_msgs::msg::StopReasonArray makeStopReasonArray();
 
-  void publish();
-
 private:
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_viz_pub_;
   rclcpp::Publisher<autoware_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reason_pub_;
-  rclcpp::Node * node_;
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
   double base_link2front_;
 
   std::shared_ptr<geometry_msgs::msg::Pose> stop_pose_ptr_;

obstacle_stop_planner_refine/include/obstacle_stop_planner/node.hpp

@@ -15,61 +15,70 @@
 #define OBSTACLE_STOP_PLANNER__NODE_HPP_
 
 #include <memory>
+#include <vector>
 
 #include "rclcpp/rclcpp.hpp"
 
 #include "obstacle_stop_planner/visibility_control.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
-#include "autoware_planning_msgs/msg/expand_stop_range.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
 #include "obstacle_stop_planner/obstacle_stop_planner.hpp"
+#include "obstacle_stop_planner/util/parameter_helper.hpp"
 
 namespace obstacle_stop_planner
 {
+using PointCloud2 = sensor_msgs::msg::PointCloud2;
+using Trajectory = autoware_planning_msgs::msg::Trajectory;
+using TwistStamped = geometry_msgs::msg::TwistStamped;
+using DynamicObjectArray = autoware_perception_msgs::msg::DynamicObjectArray;
+using MarkerArray = visualization_msgs::msg::MarkerArray;
+using StopReasonArray = autoware_planning_msgs::msg::StopReasonArray;
+using Float32MultiArrayStamped = autoware_debug_msgs::msg::Float32MultiArrayStamped;
+using Point3d = autoware_utils::Point3d;
+
 class OBSTACLE_STOP_PLANNER_PUBLIC ObstacleStopPlannerNode : public rclcpp::Node
 {
 public:
   explicit ObstacleStopPlannerNode(const rclcpp::NodeOptions & options);
 
 private:
-  /*
-   * ROS
-   */
-  // publisher and subscriber
-  rclcpp::Subscription<autoware_planning_msgs::msg::Trajectory>::SharedPtr path_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr obstacle_pointcloud_sub_;
-  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr current_velocity_sub_;
-  rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr
-    dynamic_object_sub_;
-  rclcpp::Subscription<autoware_planning_msgs::msg::ExpandStopRange>::SharedPtr
-    expand_stop_range_sub_;
-  rclcpp::Publisher<autoware_planning_msgs::msg::Trajectory>::SharedPtr path_pub_;
+  // Publisher
+  rclcpp::Publisher<Trajectory>::SharedPtr pub_trajectory_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr pub_debug_marker_array_;
+  rclcpp::Publisher<StopReasonArray>::SharedPtr pub_stop_reason_;
+  rclcpp::Publisher<Float32MultiArrayStamped>::SharedPtr pub_debug_acc_;
+
+  // Subscriber
+  rclcpp::Subscription<Trajectory>::SharedPtr sub_trajectory_;
+  rclcpp::Subscription<PointCloud2>::SharedPtr sub_obstacle_pointcloud_;
+  rclcpp::Subscription<TwistStamped>::SharedPtr sub_current_velocity_;
+  rclcpp::Subscription<DynamicObjectArray>::SharedPtr sub_dynamic_object_;
+
+  // Parameter callback
+  OnSetParametersCallbackHandle::SharedPtr set_param_res_;
+  rcl_interfaces::msg::SetParametersResult onParameter(
+    const std::vector<rclcpp::Parameter> & parameters);
+
+  // Parameter
+  std::shared_ptr<StopControlParameter> stop_param_;
+  std::shared_ptr<SlowDownControlParameter> slow_down_param_;
+  std::shared_ptr<AdaptiveCruiseControlParameter> acc_param_;
 
-  tf2_ros::Buffer tf_buffer_;
-  tf2_ros::TransformListener tf_listener_;
-  sensor_msgs::msg::PointCloud2::ConstSharedPtr obstacle_pointcloud_ptr_;
+  autoware_utils::SelfPoseListener self_pose_listener_ {this};
+  autoware_utils::TransformListener transform_listener_{this};
 
-  /*
-   * Parameter
-   */
-  geometry_msgs::msg::TwistStamped::ConstSharedPtr current_velocity_ptr_;
-  autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr object_ptr_;
-  rclcpp::Time prev_col_point_time_;
-  pcl::PointXYZ prev_col_point_;
-  bool pointcloud_received_;
-  bool current_velocity_reveived_;
+  PointCloud2::ConstSharedPtr obstacle_pointcloud_;
+  TwistStamped::ConstSharedPtr current_velocity_;
+  DynamicObjectArray::ConstSharedPtr object_array_;
 
   std::unique_ptr<obstacle_stop_planner::ObstacleStopPlanner> planner_;
 
-  void obstaclePointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr input_msg);
-  void pathCallback(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr input_msg);
-  void dynamicObjectCallback(
-    const autoware_perception_msgs::msg::DynamicObjectArray::ConstSharedPtr input_msg);
-  void currentVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr input_msg);
-  void externalExpandStopRangeCallback(
-    const autoware_planning_msgs::msg::ExpandStopRange::ConstSharedPtr input_msg);
+  bool isDataReady();
+  bool isTransformReady(const Trajectory & trajectory);
+  Input createInputData(const Trajectory & trajectory);
+  void onTrajectory(const Trajectory::ConstSharedPtr input_msg);
 };
 }  // namespace obstacle_stop_planner
 

obstacle_stop_planner_refine/include/obstacle_stop_planner/obstacle_point_cloud.hpp

@@ -26,7 +26,7 @@
 #include "rclcpp/logger.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
 #include "autoware_planning_msgs/msg/trajectory.hpp"
-#include "vehicle_info_util/vehicle_info.hpp"
+#include "vehicle_info_util/vehicle_info_util.hpp"
 
 namespace obstacle_stop_planner
 {
@@ -43,7 +43,7 @@ inline sensor_msgs::msg::PointCloud2::ConstSharedPtr updatePointCloud(
   pcl::fromROSMsg(*msg, *pointcloud_ptr);
 
   for (const auto & point : pointcloud_ptr->points) {
-    no_height_pointcloud_ptr->push_back(pcl::PointXYZ(point.x, point.y, 0.0));
+    no_height_pointcloud_ptr->emplace_back(point.x, point.y, 0.0);
   }
   filter.setInputCloud(no_height_pointcloud_ptr);
   filter.setLeafSize(0.05F, 0.05F, 100000.0F);
@@ -53,54 +53,10 @@ inline sensor_msgs::msg::PointCloud2::ConstSharedPtr updatePointCloud(
   return obstacle_ros_pointcloud_ptr;
 }
 
-inline static pcl::PointCloud<pcl::PointXYZ>::Ptr searchPointcloudNearTrajectory(
-  const autoware_planning_msgs::msg::Trajectory & trajectory,
-  const pcl::PointCloud<pcl::PointXYZ>::Ptr input_pointcloud_ptr,
-  const double search_radius,
-  const vehicle_info_util::VehicleInfo & param)
-{
-  pcl::PointCloud<pcl::PointXYZ>::Ptr output_pointcloud_ptr(
-    new pcl::PointCloud<pcl::PointXYZ>);
-  const double squared_radius = search_radius * search_radius;
-  for (const auto & trajectory_point : trajectory.points) {
-    const auto center_pose = getVehicleCenterFromBase(
-      trajectory_point.pose,
-      param.vehicle_length_m,
-      param.rear_overhang_m);
-
-    for (const auto & point : input_pointcloud_ptr->points) {
-      const double x = center_pose.position.x - point.x;
-      const double y = center_pose.position.y - point.y;
-      const double squared_distance = x * x + y * y;
-      if (squared_distance < squared_radius) {output_pointcloud_ptr->points.push_back(point);}
-    }
-  }
-  return output_pointcloud_ptr;
-}
-
-inline pcl::PointCloud<pcl::PointXYZ>::Ptr searchCandidateObstacle(
+inline pcl::PointCloud<pcl::PointXYZ>::Ptr transformObstacle(
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr & obstacle_ros_pointcloud_ptr,
-  const tf2_ros::Buffer & tf_buffer,
-  const autoware_planning_msgs::msg::Trajectory & trajectory,
-  const double search_radius,
-  const vehicle_info_util::VehicleInfo & param,
-  const rclcpp::Logger & logger)
+  const geometry_msgs::msg::TransformStamped & transform_stamped)
 {
-  // transform pointcloud
-  geometry_msgs::msg::TransformStamped transform_stamped;
-  try {
-    transform_stamped = tf_buffer.lookupTransform(
-      trajectory.header.frame_id, obstacle_ros_pointcloud_ptr->header.frame_id,
-      obstacle_ros_pointcloud_ptr->header.stamp, rclcpp::Duration::from_seconds(0.5));
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR_STREAM(
-      logger,
-      "[obstacle_stop_planner] Failed to look up transform from " <<
-        trajectory.header.frame_id << " to " << obstacle_ros_pointcloud_ptr->header.frame_id);
-    // do not publish path
-    return nullptr;
-  }
-
   Eigen::Matrix4f affine_matrix =
     tf2::transformToEigen(transform_stamped.transform).matrix().cast<float>();
   pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_pointcloud_pcl_ptr(
@@ -113,13 +69,7 @@ inline pcl::PointCloud<pcl::PointXYZ>::Ptr searchCandidateObstacle(
     *transformed_obstacle_pointcloud_ptr,
     affine_matrix);
 
-  // search obstacle candidate pointcloud to reduce calculation cost
-  auto obstacle_candidate_pointcloud_ptr = searchPointcloudNearTrajectory(
-    trajectory, transformed_obstacle_pointcloud_ptr,
-    search_radius,
-    param);
-  obstacle_candidate_pointcloud_ptr->header = transformed_obstacle_pointcloud_ptr->header;
-  return obstacle_candidate_pointcloud_ptr;
+  return transformed_obstacle_pointcloud_ptr;
 }
 
 }  // namespace obstacle_stop_planner