Remove PlanningRequest form simple profile interface

tesseract-robotics · Dec 14, 2024 · dfaeddb · dfaeddb
1 parent f5773e8
commit dfaeddb
Show file tree

Hide file tree

Showing 15 changed files with 61 additions and 126 deletions.
diff --git a/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h b/tesseract_motion_planners/simple/include/tesseract_motion_planners/simple/interpolation.h
@@ -46,10 +46,6 @@ namespace tesseract_planning
 /** @brief The Joint Group Instruction Information struct */
 struct JointGroupInstructionInfo
 {
-  JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
-                            const PlannerRequest& request,
-                            const tesseract_common::ManipulatorInfo& manip_info);
-
   JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
                             const tesseract_environment::Environment& env,
                             const tesseract_common::ManipulatorInfo& manip_info);
@@ -101,10 +97,6 @@ struct JointGroupInstructionInfo
 /** @brief The Kinematic Group Instruction Information struct */
 struct KinematicGroupInstructionInfo
 {
-  KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
-                                const PlannerRequest& request,
-                                const tesseract_common::ManipulatorInfo& manip_info);
-
   KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
                                 const tesseract_environment::Environment& env,
                                 const tesseract_common::ManipulatorInfo& manip_info);

diff --git a/.../tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h b/.../tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h
@@ -48,7 +48,7 @@ class SimplePlannerFixedSizeAssignPlanProfile : public SimplePlannerPlanProfile
                                             const MoveInstructionPoly& prev_seed,
                                             const MoveInstructionPoly& base_instruction,
                                             const InstructionPoly& next_instruction,
-                                            const PlannerRequest& request,
+                                            const std::shared_ptr<const tesseract_environment::Environment>& env,
                                             const tesseract_common::ManipulatorInfo& global_manip_info) const override;
 
   /** @brief The number of steps to use for freespace instruction */

diff --git a/...include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h b/...include/tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h
@@ -47,7 +47,7 @@ class SimplePlannerFixedSizePlanProfile : public SimplePlannerPlanProfile
                                             const MoveInstructionPoly& prev_seed,
                                             const MoveInstructionPoly& base_instruction,
                                             const InstructionPoly& next_instruction,
-                                            const PlannerRequest& request,
+                                            const std::shared_ptr<const tesseract_environment::Environment>& env,
                                             const tesseract_common::ManipulatorInfo& global_manip_info) const override;
 
   /** @brief The number of steps to use for freespace instruction */

diff --git a/.../include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h b/.../include/tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h
@@ -61,7 +61,7 @@ class SimplePlannerLVSNoIKPlanProfile : public SimplePlannerPlanProfile
                                             const MoveInstructionPoly& prev_seed,
                                             const MoveInstructionPoly& base_instruction,
                                             const InstructionPoly& next_instruction,
-                                            const PlannerRequest& request,
+                                            const std::shared_ptr<const tesseract_environment::Environment>& env,
                                             const tesseract_common::ManipulatorInfo& global_manip_info) const override;
 
   /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */

diff --git a/...simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h b/...simple/include/tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h
@@ -60,7 +60,7 @@ class SimplePlannerLVSPlanProfile : public SimplePlannerPlanProfile
                                             const MoveInstructionPoly& prev_seed,
                                             const MoveInstructionPoly& base_instruction,
                                             const InstructionPoly& next_instruction,
-                                            const PlannerRequest& request,
+                                            const std::shared_ptr<const tesseract_environment::Environment>& env,
                                             const tesseract_common::ManipulatorInfo& global_manip_info) const override;
 
   /** @brief The maximum joint distance, the norm of changes to all joint positions between successive steps. */

diff --git a/...planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h b/...planners/simple/include/tesseract_motion_planners/simple/profile/simple_planner_profile.h
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/fwd.h>
 #include <tesseract_command_language/fwd.h>
+#include <tesseract_environment/fwd.h>
 #include <tesseract_motion_planners/core/fwd.h>
 #include <tesseract_command_language/profile.h>
 
@@ -73,7 +74,7 @@ class SimplePlannerPlanProfile : public Profile
            const MoveInstructionPoly& prev_seed,
            const MoveInstructionPoly& base_instruction,
            const InstructionPoly& next_instruction,
-           const PlannerRequest& request,
+           const std::shared_ptr<const tesseract_environment::Environment>& env,
            const tesseract_common::ManipulatorInfo& global_manip_info) const = 0;
 
 protected:

diff --git a/tesseract_motion_planners/simple/src/interpolation.cpp b/tesseract_motion_planners/simple/src/interpolation.cpp
@@ -55,13 +55,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_planning
 {
-JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
-                                                     const PlannerRequest& request,
-                                                     const tesseract_common::ManipulatorInfo& manip_info)
-  : JointGroupInstructionInfo(plan_instruction, *request.env, manip_info)
-{
-}
-
 JointGroupInstructionInfo::JointGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
                                                      const tesseract_environment::Environment& env,
                                                      const tesseract_common::ManipulatorInfo& manip_info)
@@ -129,13 +122,6 @@ const Eigen::VectorXd& JointGroupInstructionInfo::extractJointPosition() const
   return getJointPosition(instruction.getWaypoint());
 }
 
-KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
-                                                             const PlannerRequest& request,
-                                                             const tesseract_common::ManipulatorInfo& manip_info)
-  : KinematicGroupInstructionInfo(plan_instruction, *request.env, manip_info)
-{
-}
-
 KinematicGroupInstructionInfo::KinematicGroupInstructionInfo(const MoveInstructionPoly& plan_instruction,
                                                              const tesseract_environment::Environment& env,
                                                              const tesseract_common::ManipulatorInfo& manip_info)

diff --git a/...ract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp b/...ract_motion_planners/simple/src/profile/simple_planner_fixed_size_assign_plan_profile.cpp
@@ -51,11 +51,11 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre
                                                   const MoveInstructionPoly& /*prev_seed*/,
                                                   const MoveInstructionPoly& base_instruction,
                                                   const InstructionPoly& /*next_instruction*/,
-                                                  const PlannerRequest& request,
+                                                  const std::shared_ptr<const tesseract_environment::Environment>& env,
                                                   const tesseract_common::ManipulatorInfo& global_manip_info) const
 {
-  KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info);
-  KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info);
+  KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info);
+  KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info);
 
   Eigen::MatrixXd states;
   if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
@@ -90,7 +90,7 @@ SimplePlannerFixedSizeAssignPlanProfile::generate(const MoveInstructionPoly& pre
   }
   else
   {
-    Eigen::VectorXd seed = request.env->getCurrentJointValues(info2.manip->getJointNames());
+    Eigen::VectorXd seed = env->getCurrentJointValues(info2.manip->getJointNames());
     tesseract_common::enforceLimits<double>(seed, info2.manip->getLimits().joint_limits);
 
     if (info2.instruction.isLinear())

diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_fixed_size_plan_profile.cpp
@@ -48,11 +48,11 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst
                                             const MoveInstructionPoly& /*prev_seed*/,
                                             const MoveInstructionPoly& base_instruction,
                                             const InstructionPoly& /*next_instruction*/,
-                                            const PlannerRequest& request,
+                                            const std::shared_ptr<const tesseract_environment::Environment>& env,
                                             const tesseract_common::ManipulatorInfo& global_manip_info) const
 {
-  KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info);
-  KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info);
+  KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info);
+  KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info);
 
   if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
     return interpolateJointJointWaypoint(info1, info2, linear_steps, freespace_steps);
@@ -63,7 +63,7 @@ SimplePlannerFixedSizePlanProfile::generate(const MoveInstructionPoly& prev_inst
   if (info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
     return interpolateCartJointWaypoint(info1, info2, linear_steps, freespace_steps);
 
-  return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, request.env->getState());
+  return interpolateCartCartWaypoint(info1, info2, linear_steps, freespace_steps, env->getState());
 }
 
 template <class Archive>

diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_no_ik_plan_profile.cpp
@@ -56,11 +56,11 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru
                                           const MoveInstructionPoly& /*prev_seed*/,
                                           const MoveInstructionPoly& base_instruction,
                                           const InstructionPoly& /*next_instruction*/,
-                                          const PlannerRequest& request,
+                                          const std::shared_ptr<const tesseract_environment::Environment>& env,
                                           const tesseract_common::ManipulatorInfo& global_manip_info) const
 {
-  JointGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info);
-  JointGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info);
+  JointGroupInstructionInfo info1(prev_instruction, *env, global_manip_info);
+  JointGroupInstructionInfo info2(base_instruction, *env, global_manip_info);
 
   if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
     return interpolateJointJointWaypoint(info1,
@@ -96,7 +96,7 @@ SimplePlannerLVSNoIKPlanProfile::generate(const MoveInstructionPoly& prev_instru
                                      rotation_longest_valid_segment_length,
                                      min_steps,
                                      max_steps,
-                                     request.env->getState());
+                                     env->getState());
 }
 
 template <class Archive>

diff --git a/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp b/tesseract_motion_planners/simple/src/profile/simple_planner_lvs_plan_profile.cpp
@@ -56,11 +56,11 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio
                                       const MoveInstructionPoly& /*prev_seed*/,
                                       const MoveInstructionPoly& base_instruction,
                                       const InstructionPoly& /*next_instruction*/,
-                                      const PlannerRequest& request,
+                                      const std::shared_ptr<const tesseract_environment::Environment>& env,
                                       const tesseract_common::ManipulatorInfo& global_manip_info) const
 {
-  KinematicGroupInstructionInfo info1(prev_instruction, *request.env, global_manip_info);
-  KinematicGroupInstructionInfo info2(base_instruction, *request.env, global_manip_info);
+  KinematicGroupInstructionInfo info1(prev_instruction, *env, global_manip_info);
+  KinematicGroupInstructionInfo info2(base_instruction, *env, global_manip_info);
 
   if (!info1.has_cartesian_waypoint && !info2.has_cartesian_waypoint)
     return interpolateJointJointWaypoint(info1,
@@ -96,7 +96,7 @@ SimplePlannerLVSPlanProfile::generate(const MoveInstructionPoly& prev_instructio
                                      rotation_longest_valid_segment_length,
                                      min_steps,
                                      max_steps,
-                                     request.env->getState());
+                                     env->getState());
 }
 
 template <class Archive>

diff --git a/tesseract_motion_planners/simple/src/simple_motion_planner.cpp b/tesseract_motion_planners/simple/src/simple_motion_planner.cpp
@@ -188,7 +188,8 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr
           if (!start_waypoint.as<CartesianWaypointPoly>().hasSeed())
           {
             // Run IK to find solution closest to start
-            KinematicGroupInstructionInfo info(prev_instruction, request, request.instructions.getManipulatorInfo());
+            KinematicGroupInstructionInfo info(
+                prev_instruction, *request.env, request.instructions.getManipulatorInfo());
             auto start_seed = getClosestJointSolution(info, start_state.getJointValues(manip->getJointNames()));
             start_waypoint.as<CartesianWaypointPoly>().setSeed(
                 tesseract_common::JointState(manip->getJointNames(), start_seed));
@@ -239,7 +240,7 @@ SimpleMotionPlanner::processCompositeInstruction(MoveInstructionPoly& prev_instr
                                  prev_seed,
                                  base_instruction,
                                  next_instruction,
-                                 request,
+                                 request.env,
                                  request.instructions.getManipulatorInfo());
 
       // The data for the last instruction should be unchanged with exception to seed or tolerance joint state

diff --git a/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp b/tesseract_motion_planners/simple/test/simple_planner_fixed_size_assign_position.cpp
@@ -70,9 +70,6 @@ class TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit : public ::testi
 
 TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian_AssignJointPosition)  // NOLINT
 {
-  PlannerRequest request;
-  request.env = env_;
-
   JointWaypointPoly wp1{ JointWaypoint(joint_names_, Eigen::VectorXd::Zero(7)) };
   MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
   MoveInstruction instr1_seed{ instr1 };
@@ -85,7 +82,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian
 
   SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
   std::vector<MoveInstructionPoly> move_instructions =
-      profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
+      profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo());
   EXPECT_EQ(move_instructions.size(), 10);
   for (std::size_t i = 0; i < move_instructions.size() - 1; ++i)
   {
@@ -112,9 +109,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, JointCartesian
 
 TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint_AssignJointPosition)  // NOLINT
 {
-  PlannerRequest request;
-  request.env = env_;
-
   CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
   MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
   MoveInstruction instr1_seed{ instr1 };
@@ -127,7 +121,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint
 
   SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
   std::vector<MoveInstructionPoly> move_instructions =
-      profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
+      profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo());
   EXPECT_EQ(move_instructions.size(), 10);
   for (std::size_t i = 0; i < move_instructions.size() - 1; ++i)
   {
@@ -153,9 +147,6 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianJoint
 
 TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCartesian_AssignJointPosition)  // NOLINT
 {
-  PlannerRequest request;
-  request.env = env_;
-
   CartesianWaypointPoly wp1{ CartesianWaypoint(Eigen::Isometry3d::Identity()) };
   MoveInstruction instr1(wp1, MoveInstructionType::FREESPACE, "TEST_PROFILE", manip_info_);
   MoveInstruction instr1_seed{ instr1 };
@@ -168,7 +159,7 @@ TEST_F(TesseractPlanningSimplePlannerFixedSizeAssignPositionUnit, CartesianCarte
 
   SimplePlannerFixedSizeAssignPlanProfile profile(10, 10);
   std::vector<MoveInstructionPoly> move_instructions =
-      profile.generate(instr1, instr1_seed, instr2, instr3, request, tesseract_common::ManipulatorInfo());
+      profile.generate(instr1, instr1_seed, instr2, instr3, env_, tesseract_common::ManipulatorInfo());
   auto fwd_kin = env_->getJointGroup(manip_info_.manipulator);
   Eigen::VectorXd position = env_->getCurrentJointValues(fwd_kin->getJointNames());
   EXPECT_EQ(move_instructions.size(), 10);