fix potential bug in fallback sampling

ctu-mrs · Jan 8, 2024 · b02dd55 · b02dd55
1 parent 862a148
commit b02dd55
Show file tree

Hide file tree

Showing 3 changed files with 30 additions and 26 deletions.
diff --git a/config/public/trajectory_generation.yaml b/config/public/trajectory_generation.yaml
@@ -44,8 +44,8 @@ mrs_uav_trajectory_generation:
   fallback_sampling:
 
     enabled: true
-    accel_factor: 0.75 # how fast when comparing to constraints
-    speed_factor: 0.75 # how fast when comparing to constraints
+    accel_factor: 1.00 # how fast when comparing to constraints
+    speed_factor: 1.00 # how fast when comparing to constraints
 
     # when stopping at waypoints, stop for this time
     stopping_time: 2.0 # [s]

diff --git a/src/eth_trajectory_generation/vertex.cpp b/src/eth_trajectory_generation/vertex.cpp
@@ -431,12 +431,16 @@ std::vector<double> estimateSegmentTimesBaca(const Vertex::Vector& vertices, con
 
     double max_velocity_time;
 
-    if (((distance - (2 * (v_max * v_max) / a_max)) / v_max) < 0) {
-      max_velocity_time = ((distance) / v_max);
+    const double distance_due_acceleration = a_max * pow(acceleration_time_1, 2) + a_max * pow(acceleration_time_2, 2);
+
+    if (distance > distance_due_acceleration) {
+      max_velocity_time = ((distance - distance_due_acceleration) / v_max);
     } else {
-      max_velocity_time = ((distance - (2 * (v_max * v_max) / a_max)) / v_max);
+      max_velocity_time = (distance_due_acceleration / v_max);
     }
 
+    max_velocity_time = ((distance) / v_max);
+
     /* double t = max_velocity_time + acceleration_time_1 + acceleration_time_2 + jerk_time_1 + jerk_time_2; */
     double t = max_velocity_time + acceleration_time_1 + acceleration_time_2;
 

diff --git a/src/mrs_trajectory_generation.cpp b/src/mrs_trajectory_generation.cpp
@@ -1229,13 +1229,13 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
   for (int i = 0; i < int(segment_times_baca.size()); i++) {
     initial_total_time += segment_times[i];
     initial_total_time_baca += segment_times_baca[i];
+
+    ROS_DEBUG("[MrsTrajectoryGeneration]: segment time [%d] = %.2f", i, segment_times_baca[i]);
   }
 
   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Euclidean): %.2f", initial_total_time);
   ROS_WARN("[MrsTrajectoryGeneration]: fallback: initial total time (Baca): %.2f", initial_total_time_baca);
 
-  // | --------- create an optimizer object and solve it -------- |
-
   eth_mav_msgs::EigenTrajectoryPoint::Vector states;
 
   // interpolate each segment
@@ -1250,7 +1250,7 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
 
     if (segment_time > 1e-1) {
 
-      n_samples = floor(segment_time / sampling_dt);
+      n_samples = ceil(segment_time / sampling_dt);
 
       // important
       if (n_samples > 0) {
@@ -1264,7 +1264,9 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
       interp_step = 0;
     }
 
-    // for the last segment, git the last waypoint completely
+    ROS_DEBUG("[MrsTrajectoryGeneration]: segment n_samples [%d] = %d", i, n_samples);
+
+    // for the last segment, hit the last waypoint completely
     // otherwise, it is hit as the first sample of the following segment
     if (n_samples > 0 && i == waypoints.size() - 2) {
       n_samples++;
@@ -1287,27 +1289,27 @@ std::optional<eth_mav_msgs::EigenTrajectoryPoint::Vector> MrsTrajectoryGeneratio
       // the first sample of the first waypoint
       // we should stop for a little bit to give the transition
       // from the initial cooordinates more time
-      if (((control_manager_diag.tracker_status.have_goal && i == 0) || (!control_manager_diag.tracker_status.have_goal && i == 1)) && j == 0) {
+      /* if (((control_manager_diag.tracker_status.have_goal && i == 0) || (!control_manager_diag.tracker_status.have_goal && i == 1)) && j == 0) { */
 
-        double time_to_stop = 0;
+      /*   double time_to_stop = 0; */
 
-        time_to_stop += fabs(initial_state.acceleration.x) / j_max_horizontal + fabs(initial_state.acceleration.y) / j_max_horizontal +
-                        fabs(initial_state.acceleration.z) / j_max_vertical;
+      /*   time_to_stop += fabs(initial_state.acceleration.x) / j_max_horizontal + fabs(initial_state.acceleration.y) / j_max_horizontal + */
+      /*                   fabs(initial_state.acceleration.z) / j_max_vertical; */
 
-        int samples_to_stop = int(round(1.0 * (time_to_stop / sampling_dt)));
+      /*   int samples_to_stop = int(round(1.0 * (time_to_stop / sampling_dt))); */
 
-        if (_fallback_sampling_first_waypoint_additional_stop_) {
-          if (control_manager_diag.tracker_status.have_goal) {
-            samples_to_stop += int(round(1.0 / sampling_dt));
-          }
-        }
+      /*   if (_fallback_sampling_first_waypoint_additional_stop_) { */
+      /*     if (control_manager_diag.tracker_status.have_goal) { */
+      /*       samples_to_stop += int(round(1.0 / sampling_dt)); */
+      /*     } */
+      /*   } */
 
-        ROS_DEBUG("[MrsTrajectoryGeneration]: pre-inserting %d samples of the first point", samples_to_stop);
+      /*   ROS_DEBUG("[MrsTrajectoryGeneration]: pre-inserting %d samples of the first point", samples_to_stop); */
 
-        for (int k = 0; k < samples_to_stop; k++) {
-          states.push_back(eth_point);
-        }
-      }
+      /*   for (int k = 0; k < samples_to_stop; k++) { */
+      /*     states.push_back(eth_point); */
+      /*   } */
+      /* } */
 
       if (j == 0 && i > 0 && waypoints[i].stop_at) {
 
@@ -1640,8 +1642,6 @@ std::optional<mrs_msgs::Path> MrsTrajectoryGeneration::transformPath(const mrs_m
     }
   }
 
-  //}
-
   return path_out;
 }