fix(autoware_freespace_planner, autoware_freespace_planning_algorithm…

…s): modify freespace planner to use node clock instead of system clock (#9152)

* Modified the autoware_freespace_planner and autoware_freespace_planning_algorithms packages to use the node clock instead of rclcpp detached clock. This allows the module to make use of sim time. Previously during simulation the parking trajectory would have system time in trajectory header messages causing downstream issues like non-clearance of trajectory buffers in motion planning based on elapsed time.

* style(pre-commit): autofix

* Updated the freespace planner instantiation call in the path planning modules

* style(pre-commit): autofix

* Updated tests for the utility functions

* style(pre-commit): autofix

---------

Co-authored-by: Steven Brills <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/autoware_freespace_planner/include/autoware/freespace_planner/utils.hpp

@@ -58,13 +58,15 @@ size_t get_next_target_index(
   const size_t current_target_index);
 
 Trajectory get_partial_trajectory(
-  const Trajectory & trajectory, const size_t start_index, const size_t end_index);
+  const Trajectory & trajectory, const size_t start_index, const size_t end_index,
+  const rclcpp::Clock::SharedPtr clock);
 
 Trajectory create_trajectory(
   const PoseStamped & current_pose, const PlannerWaypoints & planner_waypoints,
   const double & velocity);
 
-Trajectory create_stop_trajectory(const PoseStamped & current_pose);
+Trajectory create_stop_trajectory(
+  const PoseStamped & current_pose, const rclcpp::Clock::SharedPtr clock);
 
 Trajectory create_stop_trajectory(const Trajectory & trajectory);
 

planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp

@@ -168,8 +168,8 @@ bool FreespacePlannerNode::checkCurrentTrajectoryCollision()
   const size_t nearest_index_partial = autoware::motion_utils::findNearestIndex(
     partial_trajectory_.points, current_pose_.pose.position);
   const size_t end_index_partial = partial_trajectory_.points.size() - 1;
-  const auto forward_trajectory =
-    utils::get_partial_trajectory(partial_trajectory_, nearest_index_partial, end_index_partial);
+  const auto forward_trajectory = utils::get_partial_trajectory(
+    partial_trajectory_, nearest_index_partial, end_index_partial, get_clock());
 
   const bool is_obs_found =
     algo_->hasObstacleOnTrajectory(utils::trajectory_to_pose_array(forward_trajectory));
@@ -316,7 +316,7 @@ void FreespacePlannerNode::onTimer()
     // stops.
     if (!is_new_parking_cycle_) {
       const auto stop_trajectory = partial_trajectory_.points.empty()
-                                     ? utils::create_stop_trajectory(current_pose_)
+                                     ? utils::create_stop_trajectory(current_pose_, get_clock())
                                      : utils::create_stop_trajectory(partial_trajectory_);
       trajectory_pub_->publish(stop_trajectory);
       debug_pose_array_pub_->publish(utils::trajectory_to_pose_array(stop_trajectory));
@@ -352,7 +352,7 @@ void FreespacePlannerNode::onTimer()
   // Update partial trajectory
   updateTargetIndex();
   partial_trajectory_ =
-    utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_);
+    utils::get_partial_trajectory(trajectory_, prev_target_index_, target_index_, get_clock());
 
   // Publish messages
   trajectory_pub_->publish(partial_trajectory_);

planning/autoware_freespace_planner/src/autoware_freespace_planner/utils.cpp

@@ -87,11 +87,12 @@ size_t get_next_target_index(
 }
 
 Trajectory get_partial_trajectory(
-  const Trajectory & trajectory, const size_t start_index, const size_t end_index)
+  const Trajectory & trajectory, const size_t start_index, const size_t end_index,
+  const rclcpp::Clock::SharedPtr clock)
 {
   Trajectory partial_trajectory;
   partial_trajectory.header = trajectory.header;
-  partial_trajectory.header.stamp = rclcpp::Clock().now();
+  partial_trajectory.header.stamp = clock->now();
 
   partial_trajectory.points.reserve(trajectory.points.size());
   for (size_t i = start_index; i <= end_index; ++i) {
@@ -134,12 +135,13 @@ Trajectory create_trajectory(
   return trajectory;
 }
 
-Trajectory create_stop_trajectory(const PoseStamped & current_pose)
+Trajectory create_stop_trajectory(
+  const PoseStamped & current_pose, const rclcpp::Clock::SharedPtr clock)
 {
   PlannerWaypoints waypoints;
   PlannerWaypoint waypoint;
 
-  waypoints.header.stamp = rclcpp::Clock().now();
+  waypoints.header.stamp = clock->now();
   waypoints.header.frame_id = current_pose.header.frame_id;
   waypoint.pose.header = waypoints.header;
   waypoint.pose.pose = current_pose.pose;

planning/autoware_freespace_planner/test/test_freespace_planner.cpp

@@ -134,7 +134,8 @@ class TestFreespacePlanner : public ::testing::Test
     freespace_planner_->reversing_indices_ = reversing_indices;
     freespace_planner_->partial_trajectory_ =
       autoware::freespace_planner::utils::get_partial_trajectory(
-        trajectory_, 0, reversing_indices.front());
+        trajectory_, 0, reversing_indices.front(),
+        std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME));
     freespace_planner_->current_pose_.pose = trajectory_.points.front().pose;
 
     if (colliding) {
@@ -161,7 +162,8 @@ class TestFreespacePlanner : public ::testing::Test
       trajectory_.points.size(), reversing_indices, freespace_planner_->prev_target_index_);
     freespace_planner_->partial_trajectory_ =
       autoware::freespace_planner::utils::get_partial_trajectory(
-        trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_);
+        trajectory_, freespace_planner_->prev_target_index_, freespace_planner_->target_index_,
+        std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME));
 
     Odometry odom;
     odom.pose.pose = trajectory_.points.front().pose;

planning/autoware_freespace_planner/test/test_freespace_planner_utils.cpp

@@ -229,17 +229,17 @@ TEST(FreespacePlannerUtilsTest, testGetPartialTrajectory)
     autoware::freespace_planner::utils::get_reversing_indices(trajectory);
 
   ASSERT_EQ(reversing_indices.size(), 2ul);
-
   auto partial_traj = autoware::freespace_planner::utils::get_partial_trajectory(
-    trajectory, 0ul, reversing_indices.front());
+    trajectory, 0ul, reversing_indices.front(), std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME));
   ASSERT_FALSE(partial_traj.points.empty());
   auto expected_size = reversing_indices.front() + 1ul;
   EXPECT_EQ(partial_traj.points.size(), expected_size);
   EXPECT_TRUE(partial_traj.points.front().longitudinal_velocity_mps > 0.0);
   EXPECT_FLOAT_EQ(partial_traj.points.back().longitudinal_velocity_mps, 0.0);
 
   partial_traj = autoware::freespace_planner::utils::get_partial_trajectory(
-    trajectory, reversing_indices.front(), reversing_indices.back());
+    trajectory, reversing_indices.front(), reversing_indices.back(),
+    std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME));
   ASSERT_FALSE(partial_traj.points.empty());
   expected_size = reversing_indices.back() - reversing_indices.front() + 1ul;
   EXPECT_EQ(partial_traj.points.size(), expected_size);
@@ -271,7 +271,8 @@ TEST(FreespacePlannerUtilsTest, testCreateStopTrajectory)
   geometry_msgs::msg::PoseStamped current_pose;
   current_pose.pose.position.x = 1.0;
 
-  auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(current_pose);
+  auto stop_traj = autoware::freespace_planner::utils::create_stop_trajectory(
+    current_pose, std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME));
   EXPECT_EQ(stop_traj.points.size(), 1ul);
   if (!stop_traj.points.empty()) {
     EXPECT_DOUBLE_EQ(stop_traj.points.front().pose.position.x, 1.0);

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp

@@ -158,8 +158,11 @@ class AbstractPlanningAlgorithm
 {
 public:
   AbstractPlanningAlgorithm(
-    const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape)
-  : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape)
+    const PlannerCommonParam & planner_common_param, const rclcpp::Clock::SharedPtr & clock,
+    const VehicleShape & collision_vehicle_shape)
+  : planner_common_param_(planner_common_param),
+    collision_vehicle_shape_(collision_vehicle_shape),
+    clock_(clock)
   {
     planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
     collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
@@ -168,8 +171,11 @@ class AbstractPlanningAlgorithm
 
   AbstractPlanningAlgorithm(
     const PlannerCommonParam & planner_common_param,
-    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0)
-  : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin)
+    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,
+    const rclcpp::Clock::SharedPtr & clock, const double margin = 0.0)
+  : planner_common_param_(planner_common_param),
+    collision_vehicle_shape_(vehicle_info, margin),
+    clock_(clock)
   {
     planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
     collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
@@ -270,6 +276,9 @@ class AbstractPlanningAlgorithm
   PlannerCommonParam planner_common_param_;
   VehicleShape collision_vehicle_shape_;
 
+  // Pointer to the parent Node
+  rclcpp::Clock::SharedPtr clock_;
+
   // costmap as occupancy grid
   nav_msgs::msg::OccupancyGrid costmap_;
 

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp

@@ -98,6 +98,9 @@ class AstarSearch : public AbstractPlanningAlgorithm
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
     const AstarParam & astar_param);
 
+  AstarSearch(
+    const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
+    const AstarParam & astar_param, const rclcpp::Clock::SharedPtr & clock);
   AstarSearch(
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
     rclcpp::Node & node)
@@ -113,7 +116,8 @@ class AstarSearch : public AbstractPlanningAlgorithm
         node.declare_parameter<double>("astar.distance_heuristic_weight"),
         node.declare_parameter<double>("astar.smoothness_weight"),
         node.declare_parameter<double>("astar.obstacle_distance_weight"),
-        node.declare_parameter<double>("astar.goal_lat_distance_weight")})
+        node.declare_parameter<double>("astar.goal_lat_distance_weight")},
+      node.get_clock())
   {
   }
 

planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/rrtstar.hpp

@@ -39,7 +39,7 @@ class RRTStar : public AbstractPlanningAlgorithm
 public:
   RRTStar(
     const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape,
-    const RRTStarParam & rrtstar_param);
+    const RRTStarParam & rrtstar_param, const rclcpp::Clock::SharedPtr & clock);
 
   RRTStar(
     const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape,
@@ -51,7 +51,8 @@ class RRTStar : public AbstractPlanningAlgorithm
         node.declare_parameter<bool>("rrtstar.use_informed_sampling"),
         node.declare_parameter<double>("rrtstar.max_planning_time"),
         node.declare_parameter<double>("rrtstar.neighbor_radius"),
-        node.declare_parameter<double>("rrtstar.margin")})
+        node.declare_parameter<double>("rrtstar.margin")},
+      node.get_clock())
   {
   }
 

planning/autoware_freespace_planning_algorithms/src/astar_search.cpp

@@ -67,7 +67,34 @@ Pose calcRelativePose(const Pose & base_pose, const Pose & pose)
 AstarSearch::AstarSearch(
   const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
   const AstarParam & astar_param)
-: AbstractPlanningAlgorithm(planner_common_param, collision_vehicle_shape),
+: AbstractPlanningAlgorithm(
+    planner_common_param, std::make_shared<rclcpp::Clock>(RCL_ROS_TIME), collision_vehicle_shape),
+  astar_param_(astar_param),
+  goal_node_(nullptr),
+  use_reeds_shepp_(true)
+{
+  steering_resolution_ =
+    collision_vehicle_shape_.max_steering / planner_common_param_.turning_steps;
+  heading_resolution_ = 2.0 * M_PI / planner_common_param_.theta_size;
+
+  const double avg_steering =
+    steering_resolution_ + (collision_vehicle_shape_.max_steering - steering_resolution_) / 2.0;
+  avg_turning_radius_ =
+    kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering);
+
+  is_backward_search_ = astar_param_.search_method == "backward";
+
+  min_expansion_dist_ = astar_param_.expansion_distance;
+  max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_;
+
+  near_goal_dist_ =
+    std::max(astar_param.near_goal_distance, planner_common_param.longitudinal_goal_range);
+}
+
+AstarSearch::AstarSearch(
+  const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
+  const AstarParam & astar_param, const rclcpp::Clock::SharedPtr & clock)
+: AbstractPlanningAlgorithm(planner_common_param, clock, collision_vehicle_shape),
   astar_param_(astar_param),
   goal_node_(nullptr),
   use_reeds_shepp_(true)
@@ -399,7 +426,7 @@ double AstarSearch::getLatDistanceCost(const Pose & pose) const
 void AstarSearch::setPath(const AstarNode & goal_node)
 {
   std_msgs::msg::Header header;
-  header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
+  header.stamp = clock_->now();
   header.frame_id = costmap_.header.frame_id;
 
   // From the goal node to the start node

planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp

@@ -26,13 +26,14 @@ rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg)
 
 RRTStar::RRTStar(
   const PlannerCommonParam & planner_common_param, const VehicleShape & original_vehicle_shape,
-  const RRTStarParam & rrtstar_param)
+  const RRTStarParam & rrtstar_param, const rclcpp::Clock::SharedPtr & clock)
 : AbstractPlanningAlgorithm(
-    planner_common_param, VehicleShape(
-                            original_vehicle_shape.length + 2 * rrtstar_param.margin,
-                            original_vehicle_shape.width + 2 * rrtstar_param.margin,
-                            original_vehicle_shape.base_length, original_vehicle_shape.max_steering,
-                            original_vehicle_shape.base2back + rrtstar_param.margin)),
+    planner_common_param, clock,
+    VehicleShape(
+      original_vehicle_shape.length + 2 * rrtstar_param.margin,
+      original_vehicle_shape.width + 2 * rrtstar_param.margin, original_vehicle_shape.base_length,
+      original_vehicle_shape.max_steering,
+      original_vehicle_shape.base2back + rrtstar_param.margin)),
   rrtstar_param_(rrtstar_param)
 {
   if (rrtstar_param_.margin <= 0) {
@@ -129,7 +130,7 @@ bool RRTStar::hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & traj
 void RRTStar::setRRTPath(const std::vector<rrtstar_core::Pose> & waypoints)
 {
   std_msgs::msg::Header header;
-  header.stamp = rclcpp::Clock(RCL_ROS_TIME).now();
+  header.stamp = clock_->now();
   header.frame_id = costmap_.header.frame_id;
 
   waypoints_.header = header;

planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp

@@ -231,7 +231,9 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
     obstacle_distance_weight,
     goal_lat_distance_weight};
 
-  auto algo = std::make_unique<fpa::AstarSearch>(planner_common_param, vehicle_shape, astar_param);
+  auto clock_shrd_ptr = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto algo = std::make_unique<fpa::AstarSearch>(
+    planner_common_param, vehicle_shape, astar_param, clock_shrd_ptr);
   return algo;
 }
 
@@ -244,7 +246,10 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_rrtstar(bool informed,
   const double margin = 0.2;
   const double max_planning_time = 200;
   const auto rrtstar_param = fpa::RRTStarParam{update, informed, max_planning_time, mu, margin};
-  auto algo = std::make_unique<fpa::RRTStar>(planner_common_param, vehicle_shape, rrtstar_param);
+
+  auto clock_shrd_ptr = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
+  auto algo = std::make_unique<fpa::RRTStar>(
+    planner_common_param, vehicle_shape, rrtstar_param, clock_shrd_ptr);
   return algo;
 }
 

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/freespace_pull_over.cpp

@@ -48,11 +48,12 @@ FreespacePullOver::FreespacePullOver(
     vehicle_info, parameters.vehicle_shape_margin);
   if (parameters.freespace_parking_algorithm == "astar") {
     planner_ = std::make_unique<AstarSearch>(
-      parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters);
+      parameters.freespace_parking_common_parameters, vehicle_shape, parameters.astar_parameters,
+      node.get_clock());
   } else if (parameters.freespace_parking_algorithm == "rrtstar") {
     planner_ = std::make_unique<RRTStar>(
-      parameters.freespace_parking_common_parameters, vehicle_shape,
-      parameters.rrt_star_parameters);
+      parameters.freespace_parking_common_parameters, vehicle_shape, parameters.rrt_star_parameters,
+      node.get_clock());
   }
 }
 

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp

@@ -38,12 +38,13 @@ FreespacePullOut::FreespacePullOut(
   if (parameters.freespace_planner_algorithm == "astar") {
     use_back_ = parameters.astar_parameters.use_back;
     planner_ = std::make_unique<AstarSearch>(
-      parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters);
+      parameters.freespace_planner_common_parameters, vehicle_shape, parameters.astar_parameters,
+      node.get_clock());
   } else if (parameters.freespace_planner_algorithm == "rrtstar") {
     use_back_ = true;  // no option for disabling back in rrtstar
     planner_ = std::make_unique<RRTStar>(
-      parameters.freespace_planner_common_parameters, vehicle_shape,
-      parameters.rrt_star_parameters);
+      parameters.freespace_planner_common_parameters, vehicle_shape, parameters.rrt_star_parameters,
+      node.get_clock());
   }
 }