feature: executePlanToPose

Author: IPA-KUT-CL

include/ur_manipulation/moveit_custom_api.hpp

@@ -55,6 +55,7 @@ class MoveitCustomApi
   void executeCartesianTrajForWaypoints(std::vector<geometry_msgs::Pose> waypoints, double eef = 0.001, double jump_thresh = 0.0);
   void executeCartesianTrajtoPose(geometry_msgs::Pose target, std::string label);
   void adjustTrajectoryToFixTimeSequencing(moveit_msgs::RobotTrajectory &trajectory);
+  void executePlanToPose(geometry_msgs::Pose target, std::string label);
 
 private:
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

src/moveit_custom_api.cpp

@@ -666,3 +666,20 @@ moveit::planning_interface::MoveGroupInterface::Plan MoveitCustomApi::
   }
   return my_plan;
 }
+
+void MoveitCustomApi::executePlanToPose(geometry_msgs::Pose target, std::string label)
+{
+  int trial = 0;
+  while (trial < max_trials)
+  {
+    if (moveGroupExecutePlan(getPlanToPoseTarget(target, 3, label)))
+    {
+      return;
+    }
+    ROS_ERROR_STREAM("Execution to " << label << " trial " << trial++ << " failed. Reattempting");
+    failure_counter_++;
+  }
+  ROS_ERROR_STREAM("Maxx execution attempts reached, aborting program");
+  ros::shutdown();
+  exit(-1);
+}