Remove debug prints

moveit · Feb 21, 2023 · 0b9fdf4 · 0b9fdf4
1 parent 445c7e8
commit 0b9fdf4
Showing 1 changed file with 7 additions and 12 deletions.
diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp
@@ -301,18 +301,13 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
   {
     for (size_t waypoint_idx = 0; waypoint_idx < num_waypoints - 1; ++waypoint_idx)
     {
-      std::cerr << "WAYPOINT" << std::endl;
       moveit::core::RobotStatePtr next_waypoint = trajectory.getWayPointPtr(waypoint_idx + 1);
-      next_waypoint->printStateInfo(std::cerr);
 
       getNextRuckigInput(trajectory.getWayPointPtr(waypoint_idx), next_waypoint, group, ruckig_input);
 
       // Run Ruckig
       ruckig_result = ruckig_ptr->update(ruckig_input, ruckig_output);
 
-      std::cerr << "INPUT: " << std::endl << ruckig_input.to_string() << std::endl;
-      std::cerr << "OUTPUT: " << std::endl << ruckig_output.to_string() << std::endl;
-
       // The difference between Result::Working and Result::Finished is that Finished can be reached in one
       // Ruckig timestep (constructor parameter). Both are acceptable for trajectories.
       // (The difference is only relevant for streaming mode.)
@@ -321,18 +316,18 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory,
       if ((waypoint_idx == num_waypoints - 2) &&
           (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished))
       {
-        std::cerr << "Final waypoint duration: " << ruckig_output.trajectory.get_duration() << std::endl;
         trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
         smoothing_complete = true;
         break;
       }
 
-      // If successful, on to the next waypoint
-      if (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)
-      {
-        trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
-        continue;
-      }
+      // TODO(andyz): why doesn't this work?
+      // // If successful, on to the next waypoint
+      // if (ruckig_result == ruckig::Result::Working || ruckig_result == ruckig::Result::Finished)
+      // {
+      //   trajectory.setWayPointDurationFromPrevious(waypoint_idx + 1, ruckig_output.trajectory.get_duration());
+      //   continue;
+      // }
 
       // Extend the trajectory duration if Ruckig could not reach the waypoint successfully
       if (ruckig_result != ruckig::Result::Working && ruckig_result != ruckig::Result::Finished)