added possibility to optionally optimize time during compute cartesian path ‎️‍🔥

moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp

@@ -176,12 +176,18 @@ bool MoveGroupCartesianPathService::computeService(
 
           // time trajectory
           // \todo optionally compute timing to move the eef with constant speed
-          trajectory_processing::TimeOptimalTrajectoryGeneration time_param;
-          time_param.computeTimeStamps(rt, 1.0);
+          if (req->time_optimize)
+          {
+            auto orig_point_count = rt.getWayPointCount();
+            trajectory_processing::TimeOptimalTrajectoryGeneration time_param;
+            time_param.computeTimeStamps(rt, 1.0);
+            RCLCPP_INFO(LOGGER, "Optimized trajectory with %u points to %u points", (unsigned int)orig_point_count,
+                        (unsigned int)rt.getWayPointCount());
+          }
 
           rt.getRobotTrajectoryMsg(res->solution);
           RCLCPP_INFO(LOGGER, "Computed Cartesian path with %u points (followed %lf%% of requested trajectory)",
-                      (unsigned int)traj.size(), res->fraction * 100.0);
+                      (unsigned int)rt.getWayPointCount(), res->fraction * 100.0);
           if (display_computed_paths_ && rt.getWayPointCount() > 0)
           {
             moveit_msgs::msg::DisplayTrajectory disp;

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -739,10 +739,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       Collisions are avoided if \e avoid_collisions is set to true. If collisions cannot be avoided, the function fails.
       Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the
      waypoints.
-      Return -1.0 in case of error. */
+     Time optimization is performed if \e time_optimize is set to true. Else the trajectory will be returned
+     unoptimized.
+     Return -1.0 in case of error. */
   double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
                               double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
-                              bool avoid_collisions = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
+                              bool avoid_collisions = true, bool time_optimize = true,
+                              moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
 
   /** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters
       between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the
@@ -755,11 +758,13 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
      met, the function fails.
       Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the
      waypoints.
+    Time optimization is performed if \e time_optimize is set to true. Else the trajectory will be returned
+     unoptimized.
       Return -1.0 in case of error. */
   double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
                               double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
                               const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions = true,
-                              moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
+                              bool time_optimize = true, moveit_msgs::msg::MoveItErrorCodes* error_code = nullptr);
 
   /** \brief Stop any trajectory execution, if one is active */
   void stop();

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -950,7 +950,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
   double computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double step,
                               double jump_threshold, moveit_msgs::msg::RobotTrajectory& msg,
                               const moveit_msgs::msg::Constraints& path_constraints, bool avoid_collisions,
-                              moveit_msgs::msg::MoveItErrorCodes& error_code)
+                              bool time_optimize, moveit_msgs::msg::MoveItErrorCodes& error_code)
   {
     auto req = std::make_shared<moveit_msgs::srv::GetCartesianPath::Request>();
     moveit_msgs::srv::GetCartesianPath::Response::SharedPtr response;
@@ -969,6 +969,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     req->path_constraints = path_constraints;
     req->avoid_collisions = avoid_collisions;
     req->link_name = getEndEffectorLink();
+    req->time_optimize = time_optimize;
 
     auto future_response = cartesian_path_service_->async_send_request(req);
     if (future_response.valid())
@@ -1594,28 +1595,30 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan)
 
 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
                                                 double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
-                                                bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
+                                                bool avoid_collisions, bool time_optimize,
+                                                moveit_msgs::msg::MoveItErrorCodes* error_code)
 {
   moveit_msgs::msg::Constraints path_constraints_tmp;
   return computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints_tmp, avoid_collisions,
-                              error_code);
+                              time_optimize, error_code);
 }
 
 double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
                                                 double jump_threshold, moveit_msgs::msg::RobotTrajectory& trajectory,
                                                 const moveit_msgs::msg::Constraints& path_constraints,
-                                                bool avoid_collisions, moveit_msgs::msg::MoveItErrorCodes* error_code)
+                                                bool avoid_collisions, bool time_optimize,
+                                                moveit_msgs::msg::MoveItErrorCodes* error_code)
 {
   if (error_code)
   {
     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
-                                       avoid_collisions, *error_code);
+                                       avoid_collisions, time_optimize, *error_code);
   }
   else
   {
     moveit_msgs::msg::MoveItErrorCodes error_code_tmp;
     return impl_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory, path_constraints,
-                                       avoid_collisions, error_code_tmp);
+                                       avoid_collisions, time_optimize, error_code_tmp);
   }
 }