fix(freespace_planning_algorithms): throw error when no waypoints sto…

…red (autowarefoundation#2505)

* fix(freespace_planning_algorithms): throw error when no waypoints stored

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* test: continue if plan is failed

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* Return waypoints even when solution is not found

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>

* ci(pre-commit): autofix

---------

Signed-off-by: Hirokazu Ishida <h-ishida@jsk.imi.i.u-tokyo.ac.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com>

planning/freespace_planning_algorithms/include/freespace_planning_algorithms/abstract_algorithm.hpp

@@ -111,6 +111,7 @@ class AbstractPlanningAlgorithm
     const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose) = 0;
   virtual bool hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & trajectory) const;
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
+
   virtual ~AbstractPlanningAlgorithm() {}
 
 protected:

planning/freespace_planning_algorithms/src/abstract_algorithm.cpp

@@ -89,6 +89,9 @@ geometry_msgs::msg::Pose local2global(
 
 double PlannerWaypoints::compute_length() const
 {
+  if (waypoints.empty()) {
+    std::runtime_error("cannot compute cost because waypoint has size 0");
+  }
   double total_cost = 0.0;
   for (size_t i = 0; i < waypoints.size() - 1; ++i) {
     const auto pose_a = waypoints.at(i);

planning/freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp

@@ -270,8 +270,6 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
   for (size_t i = 0; i < goal_poses.size(); ++i) {
     const auto goal_pose = goal_poses.at(i);
 
-    bool success_local = true;
-
     algo->setMap(costmap_msg);
     double msec;
     double cost;
@@ -284,7 +282,9 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
       for (size_t j = 0; j < N_mc; j++) {
         const rclcpp::Time begin = clock.now();
         if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) {
-          success_local = false;
+          success_all = false;
+          std::cout << "plan fail" << std::endl;
+          continue;
         }
         const rclcpp::Time now = clock.now();
         time_sum += (now - begin).seconds() * 1000.0;
@@ -295,19 +295,18 @@ bool test_algorithm(enum AlgorithmType algo_type, bool dump_rosbag = false)
 
     } else {
       const rclcpp::Time begin = clock.now();
-      success_local = algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose));
+      if (!algo->makePlan(create_pose_msg(start_pose), create_pose_msg(goal_pose))) {
+        success_all = false;
+        std::cout << "plan fail" << std::endl;
+        continue;
+      }
       const rclcpp::Time now = clock.now();
       msec = (now - begin).seconds() * 1000.0;
       cost = algo->getWaypoints().compute_length();
     }
 
-    if (success_local) {
-      std::cout << "plan success : " << msec << "[msec]"
-                << ", solution cost : " << cost << std::endl;
-    } else {
-      success_all = false;
-      std::cout << "plan fail : " << msec << "[msec]" << std::endl;
-    }
+    std::cout << "plan success : " << msec << "[msec]"
+              << ", solution cost : " << cost << std::endl;
     const auto result = algo->getWaypoints();
     geometry_msgs::msg::PoseArray trajectory;
     for (const auto & pose : result.waypoints) {