SYSU-STAR · yzhangec · Dec 12, 2024
diff --git a/falcon_planner/exploration_manager/config/rviz.rviz b/falcon_planner/exploration_manager/config/rviz.rviz
@@ -4,7 +4,6 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Trajectory1
         - /Trajectory1/bspline_traj1/Namespaces1
         - /Trajectory1/position_cmd1
         - /Trajectory1/position_cmd1/Namespaces1
@@ -165,7 +164,6 @@ Visualization Manager:
           Marker Topic: /planning/position_cmd_vis
           Name: position_cmd
           Namespaces:
-            current_cmd: true
             current_fov: true
           Queue Size: 100
           Value: true
@@ -907,25 +905,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 22.538219451904297
+      Distance: 38.2598991394043
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: -0.7072279453277588
-        Y: -8.028024673461914
-        Z: -4.76837158203125e-06
-      Focal Shape Fixed Size: false
+        X: 3
+        Y: -7.5
+        Z: -5.5
+      Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.41039881110191345
+      Pitch: 0.4000000059604645
       Target Frame: <Fixed Frame>
-      Yaw: 3.030402660369873
+      Yaw: 3.5999999046325684
     Saved:
       - Class: rviz/ThirdPersonFollower
         Distance: 24

diff --git a/falcon_planner/exploration_manager/src/exploration_manager.cpp b/falcon_planner/exploration_manager/src/exploration_manager.cpp
@@ -575,7 +575,7 @@ int ExplorationManager::planExploreMotionHGrid(const Vector3d &pos, const Vector
 
     double sop_time = (ros::Time::now() - t1).toSec();
     ROS_INFO("[ExplorationManager] SOP time: %.2f ms", sop_time * 1000.0);
-    CHECK_LE(sop_time, 0.5) << "SOP solver internal error detected, solver blocked with unknown "
+    CHECK_LE(sop_time, 1.0) << "SOP solver internal error detected, solver blocked with unknown "
                                "error. Please restart the planner";
     ee_->sop_times_.push_back(make_pair(sop_cost_matrix_time, sop_time));
 

diff --git a/falcon_planner/exploration_utils/resource/coverage_path.tsp b/falcon_planner/exploration_utils/resource/coverage_path.tsp
diff --git a/falcon_planner/exploration_utils/resource/coverage_path.txt b/falcon_planner/exploration_utils/resource/coverage_path.txt
@@ -1,94 +1,14 @@
-NAME : coverage_path.38284.tour
-COMMENT : Length = 38284
-COMMENT : Found by LKH [Keld Helsgaun] Sat Dec  7 15:54:38 2024
+NAME : coverage_path.2143.tour
+COMMENT : Length = 2143
+COMMENT : Found by LKH [Keld Helsgaun] Thu Dec 12 14:15:52 2024
 TYPE : TOUR
-DIMENSION : 86
+DIMENSION : 6
 TOUR_SECTION
 1
-42
-44
-45
-70
-69
-68
-71
-75
-78
-81
-84
-85
-86
-83
-82
-79
-80
-77
-76
-72
-74
-73
-48
-52
-53
-49
-47
-43
-41
-46
-50
-51
-56
-54
-60
-65
-62
-66
-67
-64
-58
-57
-63
-61
-55
-59
-33
-34
-38
-39
-35
-37
-40
-36
-29
-31
-30
-32
-27
-28
-24
-23
-22
-26
-25
-21
-15
-16
-18
-19
-17
-20
 4
-7
-6
-3
-2
 5
-8
-11
-12
-13
-10
-9
-14
+2
+3
+6
 -1
 EOF
diff --git a/falcon_planner/fast_planner/src/planner_manager.cpp b/falcon_planner/fast_planner/src/planner_manager.cpp
@@ -84,32 +84,54 @@ void FastPlannerManager::planExplorationPositionTraj(const vector<Eigen::Vector3
     pos.row(i) = tour[i];
 
   Eigen::VectorXd times(pt_num - 1); // time for each segment on tour
+  Eigen::Vector3d first_seg_vel;
   for (int i = 0; i < pt_num - 1; ++i) {
-    times(i) = (pos.row(i + 1) - pos.row(i)).norm() / (pp_.max_vel_);
-
-    if (i == 0) {
-      // consider cur_vel for first segment
-      Vector3d dir;
-      dir = (pos.row(1) - pos.row(0)).normalized();
-      double vc = cur_vel.dot(dir);
-      double t = pow(pp_.max_vel_ - fabs(vc), 2) / (2 * pp_.max_vel_ * pp_.max_acc_);
-
-      if (vc < 0) {
-        t += 2 * fabs(vc) / pp_.max_acc_;
+    Eigen::Vector3d time_xyz;
+    Eigen::Vector3d dir = (pos.row(i + 1) - pos.row(i)).normalized();
+    for (int j = 0; j < 3; ++j) {
+      double len = fabs(pos(i + 1, j) - pos(i, j));
+      double len_threshold = (pow(pp_.max_vel_, 2) - pow(cur_vel(j), 2)) / (2 * pp_.max_acc_);
+      if (i == 0) {
+        // consider cur_vel for first segment
+        if (len < len_threshold) {
+          double vc = cur_vel(j);
+          double t =
+              (sqrt(pow(vc, 2) + 2 * pp_.max_acc_ * 0.5 * len) - fabs(vc)) / (pp_.max_acc_ * 0.5);
+          if (vc * dir(j) < 0) {
+            t += 2 * fabs(vc) / (pp_.max_acc_ * 0.5);
+          }
+          time_xyz(j) = t;
+        } else {
+          double vc = cur_vel(j);
+          double t = pow(pp_.max_vel_ - fabs(vc), 2) / (2 * pp_.max_vel_ * pp_.max_acc_);
+          if (vc * dir(j) < 0) {
+            t += 2 * fabs(vc) / pp_.max_acc_;
+          }
+          time_xyz(j) = fabs(pos(i + 1, j) - pos(i, j)) / (pp_.max_vel_) + t;
+        }
+        if (pos(i + 1, j) - pos(i, j) < 0) {
+          first_seg_vel(j) = cur_vel(j) - pp_.max_acc_ * time_xyz(j);
+        } else {
+          first_seg_vel(j) = cur_vel(j) + pp_.max_acc_ * time_xyz(j);
+        }
+        first_seg_vel(j) = max(-pp_.max_vel_, min(pp_.max_vel_, first_seg_vel(j)));
       }
-      times(i) += t;
-    }
-
-    if (i == pt_num - 2) {
-      // consider vel change for last segment to zero
-      double vc = pp_.max_vel_;
-      double t = pow(0 - fabs(vc), 2) / (2 * pp_.max_vel_ * pp_.max_acc_);
-
-      if (vc < 0) {
-        t += 2 * fabs(vc) / pp_.max_acc_;
+      if (i == pt_num - 2) {
+        // consider vel change for last segment to zero
+        double vc = 0.0;
+        if (pt_num == 3) {
+          vc = first_seg_vel(j);
+        } else {
+          vc = (pos(i + 1, j) - pos(i, j) < 0) ? -pp_.max_vel_ : pp_.max_vel_;
+        }
+        double t = pow(0 - fabs(vc), 2) / (2 * pp_.max_vel_ * pp_.max_acc_);
+        if (vc * dir(j) < 0) {
+          t += 2 * fabs(vc) / pp_.max_acc_;
+        }
+        time_xyz(j) += t;
       }
-      times(i) += t;
     }
+    times(i) = time_xyz.maxCoeff();
   }
 
   // Print times
@@ -127,14 +149,30 @@ void FastPlannerManager::planExplorationPositionTraj(const vector<Eigen::Vector3
 
   bool need_optimize = true;
 
-  PolynomialTraj::waypointsTraj(pos, cur_vel_bound, Eigen::Vector3d::Zero(), cur_acc_bound,
-                                Eigen::Vector3d::Zero(), times, local_data_.init_traj_poly_);
+  bool is_tour_straight_line = false;
+  if (tour.size() == 3) {
+    Eigen::Vector3d dir1 = (tour[1] - tour[0]).normalized();
+    Eigen::Vector3d dir2 = (tour[2] - tour[1]).normalized();
+    if (dir1.dot(dir2) > 1 - 1e-3) {
+      is_tour_straight_line = true;
+    }
+  }
+  if (is_tour_straight_line) {
+    PolynomialTraj::oneSegmentTraj(tour[0], tour[2], cur_vel_bound, Eigen::Vector3d::Zero(),
+                                   cur_acc_bound, Eigen::Vector3d::Zero(), times.sum(), local_data_.init_traj_poly_);
+  } else {
+    PolynomialTraj::waypointsTraj(pos, cur_vel_bound, Eigen::Vector3d::Zero(), cur_acc_bound,
+                                  Eigen::Vector3d::Zero(), times, local_data_.init_traj_poly_);
+  }
   PolynomialTraj &init_traj = local_data_.init_traj_poly_;
 
   if (tour.size() == 3 && (tour[1] - tour[0]).norm() + (tour[2] - tour[1]).norm() < 1.0) {
     ROS_WARN("[FastPlannerManager] Short path, no need to optimize.");
     need_optimize = false;
 
+    PolynomialTraj::oneSegmentTraj(tour[0], tour[2], cur_vel_bound, Eigen::Vector3d::Zero(),
+                                   cur_acc_bound, Eigen::Vector3d::Zero(), times.sum(), init_traj);
+
     // Hack for short tour glitch
     double tour_len = (tour[1] - tour[0]).norm() + (tour[2] - tour[1]).norm();
     if (init_traj.getLength() > 5.0 * tour_len) {
@@ -298,9 +336,8 @@ void FastPlannerManager::planExplorationYawWaypointsTraj(
   // Call B-spline optimization solver
   int cost_func = BsplineOptimizer::EXPLORATION_YAW_PHASE;
 
-  vector<Eigen::Vector3d> start = {Eigen::Vector3d(start_yaw3d[0], cur_yaw_vel, 0),
-                                   Eigen::Vector3d(start_yaw3d[1], 0, 0),
-                                   Eigen::Vector3d(start_yaw3d[2], 0, 0)};
+  vector<Eigen::Vector3d> start = {Eigen::Vector3d(start_yaw3d[0], start_yaw3d[1], start_yaw3d[2]),
+                                   Eigen::Vector3d(cur_yaw_vel, 0, 0), Eigen::Vector3d(0, 0, 0)};
   vector<Eigen::Vector3d> end = {Eigen::Vector3d(end_yaw3d[0], 0, 0), Eigen::Vector3d(0, 0, 0)};
   bspline_optimizers_[1]->setBoundaryStates(start, end);
   bspline_optimizers_[1]->setWaypoints(yaw_waypts, yaw_waypts_idx);

diff --git a/falcon_planner/trajectory/include/polynomial/polynomial_traj.h b/falcon_planner/trajectory/include/polynomial/polynomial_traj.h
@@ -154,6 +154,11 @@ class PolynomialTraj {
     mean_d /= double(sample_num);
   }
 
+  static void oneSegmentTraj(const Eigen::Vector3d &start_pos, const Eigen::Vector3d &end_pos,
+                             const Eigen::Vector3d &start_vel, const Eigen::Vector3d &end_vel,
+                             const Eigen::Vector3d &start_acc, const Eigen::Vector3d &end_acc,
+                             const double &time, PolynomialTraj &poly_traj);
+
   // input : position of waypoints, start/end vel and acc, segment time
   // Pos: Nx3
   static void waypointsTraj(const Eigen::MatrixXd &positions, const Eigen::Vector3d &start_vel,

diff --git a/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp b/falcon_planner/trajectory/src/polynomial/polynomial_traj.cpp
@@ -2,6 +2,42 @@
 #include <polynomial/polynomial_traj.h>
 
 namespace fast_planner {
+void PolynomialTraj::oneSegmentTraj(const Eigen::Vector3d &start_pos,
+                                    const Eigen::Vector3d &end_pos,
+                                    const Eigen::Vector3d &start_vel,
+                                    const Eigen::Vector3d &end_vel,
+                                    const Eigen::Vector3d &start_acc,
+                                    const Eigen::Vector3d &end_acc, const double &time,
+                                    PolynomialTraj &poly_traj) {
+  Eigen::MatrixXd Ab = Eigen::MatrixXd::Zero(6, 6);
+  Eigen::VectorXd Dx = Eigen::VectorXd::Zero(6);
+  Eigen::VectorXd Dy = Eigen::VectorXd::Zero(6);
+  Eigen::VectorXd Dz = Eigen::VectorXd::Zero(6);
+  double T = time;
+  // clang-format off
+  Ab << 1, 0, 0, 0, 0, 0, \
+        1, pow(T, 1), pow(T, 2), pow(T, 3), pow(T, 4), pow(T, 5), \
+        0, 1, 0, 0, 0, 0, \
+        0, 1, 2 * pow(T, 1), 3 * pow(T, 2), 4 * pow(T, 3), 5 * pow(T, 4), \
+        0, 0, 2, 0, 0, 0, \
+        0, 0, 2, 6 * pow(T, 1), 12 * pow(T, 2), 20 * pow(T, 3);
+  // clang-format on
+  Dx << start_pos(0), end_pos(0), start_vel(0), end_vel(0), start_acc(0), end_acc(0);
+  Dy << start_pos(1), end_pos(1), start_vel(1), end_vel(1), start_acc(1), end_acc(1);
+  Dz << start_pos(2), end_pos(2), start_vel(2), end_vel(2), start_acc(2), end_acc(2);
+
+  Eigen::VectorXd Px, Py, Pz;
+  Px = Eigen::VectorXd::Zero(6);
+  Py = Eigen::VectorXd::Zero(6);
+  Pz = Eigen::VectorXd::Zero(6);
+  Px = Ab.colPivHouseholderQr().solve(Dx);
+  Py = Ab.colPivHouseholderQr().solve(Dy);
+  Pz = Ab.colPivHouseholderQr().solve(Dz);
+  poly_traj.reset();
+  Polynomial poly(Px, Py, Pz, time);
+  poly_traj.addSegment(poly);
+}
+
 void PolynomialTraj::waypointsTraj(const Eigen::MatrixXd& positions, const Eigen::Vector3d& start_vel,
                                    const Eigen::Vector3d& end_vel, const Eigen::Vector3d& start_acc,
                                    const Eigen::Vector3d& end_acc, const Eigen::VectorXd& times,