remove planToPoses (C++)

UM-ARM-Lab · Aug 17, 2022 · 03f32af · 03f32af
1 parent 6d6069c
commit 03f32af
Show file tree

Hide file tree

Showing 3 changed files with 0 additions and 43 deletions.
diff --git a/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp b/jacobian_follower/include/jacobian_follower/jacobian_follower.hpp
@@ -272,7 +272,4 @@ class JacobianFollower {
       throw std::range_error(ss.str());
     }
   }
-
-  moveit_msgs::MotionPlanResponse planToPoses(std::string const &group_name, std::vector<std::string> const &ee_links,
-                                              std::vector<geometry_msgs::Pose> const &target_poses);
 };
diff --git a/jacobian_follower/src/jacobian_follower/bindings.cpp b/jacobian_follower/src/jacobian_follower/bindings.cpp
@@ -77,7 +77,6 @@ PYBIND11_MODULE(pyjacobian_follower, m) {
       .def("get_scene", &JacobianFollower::get_scene)
       .def("get_jacobian", &JacobianFollower::getJacobian)
       .def("get_base_link", &JacobianFollower::getBaseLink)
-      .def("plan_to_poses", &JacobianFollower::planToPoses)
       //
       ;
 }
diff --git a/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp b/jacobian_follower/src/jacobian_follower/jacobian_follower.cpp
@@ -1077,42 +1077,3 @@ std::string JacobianFollower::getBaseLink(std::string const &group_name) const {
   auto const jmg = model_->getJointModelGroup(group_name);
   return jmg->getCommonRoot()->getParentLinkModel()->getParentLinkModel()->getName();
 }
-
-moveit_msgs::MotionPlanResponse JacobianFollower::planToPoses(std::string const &group_name,
-                                                              std::vector<std::string> const &ee_links,
-                                                              std::vector<geometry_msgs::Pose> const &target_poses) {
-  scene_monitor_->lockSceneRead();
-  auto const &planning_scene = planning_scene::PlanningScene::clone(scene_monitor_->getPlanningScene());
-  scene_monitor_->unlockSceneRead();
-
-  std::vector<double> tolerance_pose(3, 0.01);
-  std::vector<double> tolerance_angle(3, 0.01);
-
-  planning_interface::MotionPlanRequest req;
-  planning_interface::MotionPlanResponse res;
-  req.group_name = group_name;
-  for (auto i{0u}; i <= ee_links.size(); ++i) {
-    auto const &ee_link = ee_links[i];
-    auto const &target_pose = target_poses[i];
-    geometry_msgs::PoseStamped pose;
-    pose.header.frame_id = ee_link;
-    pose.pose = target_pose;
-    ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".plan_to_poses", "ee " << ee_link);
-    auto const pose_goal =
-        kinematic_constraints::constructGoalConstraints(ee_link, pose, tolerance_pose, tolerance_angle);
-    ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".plan_to_poses", pose_goal);
-    req.goal_constraints.push_back(pose_goal);
-  }
-
-
-  ROS_DEBUG_STREAM_NAMED(LOGGER_NAME + ".plan_to_poses", "planning" << std::endl);
-  {
-    planning_scene_monitor::LockedPlanningSceneRO lscene(scene_monitor_);
-    planning_pipeline_->generatePlan(lscene, req, res);
-  }
-
-  moveit_msgs::MotionPlanResponse response_msg;
-  res.getMessage(response_msg);
-
-  return response_msg;
-}