fix(autoware_freespace_planner, autoware_freespace_planning_algorithms): modify freespace planner to use node clock instead of system clock

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 @@
 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 @@
         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

@@ -1,4 +1,4 @@
 // Copyright 2015-2019 Autoware Foundation. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -67,7 +67,34 @@
 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 @@
 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 @@
   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());
   }
 }