diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp index 2566f1db11587a86ce08940a0cb89b908eb7f9c2..1e4dbbdc66ce7b21e51622369edb07b373eab536 100644 --- a/src/util/TrajectoryUtil.cpp +++ b/src/util/TrajectoryUtil.cpp @@ -35,11 +35,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); trajectory_processing::IterativeParabolicTimeParameterization iptp{}; - ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s"); - - if (iptp.computeTimeStamps(rt)) { - ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s"); - } + iptp.computeTimeStamps(rt); if (iptp.computeTimeStamps(rt, maxVelocityFactor)) { ROS_INFO("Computed time stamps."); @@ -53,8 +49,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte } } else if (pathType == FLUID_PATH) { if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) { - ROS_ERROR("Computation of fluid path has failed."); - ROS_ERROR_STREAM("Path was motion to:\n" << targetPose); + ROS_ERROR_STREAM("Computation of fluid path has failed. Path was motion to:\n" << targetPose); } } else { ROS_ERROR("Invalid type of path provided.");