Update planning/freespace_planner/src/freespace_planner/freespace_pla…

…nner_node.cpp Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
autowarefoundation · Sep 15, 2022 · 1e80c62 · 1e80c62
1 parent f98bc4c
commit 1e80c62
Showing 1 changed file with 6 additions and 11 deletions.
diff --git a/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp b/planning/freespace_planner/src/freespace_planner/freespace_planner_node.cpp
@@ -439,17 +439,12 @@ void FreespacePlannerNode::onTimer()
   initializePlanningAlgorithm();
   if (isPlanRequired()) {
     // Stop before planning new trajectory
-    if (partial_trajectory_.points.size() > 0) {
-      const auto stop_trajectory = createStopTrajectory(partial_trajectory_);
-      trajectory_pub_->publish(stop_trajectory);
-      debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-      debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-    } else {
-      const auto stop_trajectory = createStopTrajectory(current_pose_);
-      trajectory_pub_->publish(stop_trajectory);
-      debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-      debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
-    }
+    const auto stop_trajectory = partial_trajectory_.points.empty()
+                                   ? createStopTrajectory(current_pose_)
+                                   : createStopTrajectory(partial_trajectory_);
+    trajectory_pub_->publish(stop_trajectory);
+    debug_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
+    debug_partial_pose_array_pub_->publish(trajectory2PoseArray(stop_trajectory));
 
     reset();