Add utility for getting profiles (#412)

mpowelson · web-flow · commit 4e8e44571835 · 2020-10-27T16:36:03.000-05:00
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/command_language.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/command_language.h
@@ -28,6 +28,7 @@
 
 #include <tesseract_command_language/cartesian_waypoint.h>
 #include <tesseract_command_language/composite_instruction.h>
+#include <tesseract_command_language/constants.h>
 #include <tesseract_command_language/instruction_type.h>
 #include <tesseract_command_language/joint_waypoint.h>
 #include <tesseract_command_language/manipulator_info.h>
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/composite_instruction.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/composite_instruction.h
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/core/instruction.h>
+#include <tesseract_command_language/constants.h>
 #include <tesseract_command_language/instruction_type.h>
 #include <tesseract_command_language/manipulator_info.h>
 #include <tesseract_command_language/null_instruction.h>
@@ -53,7 +54,7 @@ class CompositeInstruction
   using Ptr = std::shared_ptr<CompositeInstruction>;
   using ConstPtr = std::shared_ptr<const CompositeInstruction>;
 
-  CompositeInstruction(std::string profile = "DEFAULT",
+  CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY,
                        CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED,
                        ManipulatorInfo manipulator_info = ManipulatorInfo());
 
@@ -226,7 +227,7 @@ class CompositeInstruction
    *
    * If it has a child composite instruction it uses the child composites profile for that section
    */
-  std::string profile_{ "DEFAULT" };
+  std::string profile_{ DEFAULT_PROFILE_KEY };
 
   /** @brief The order of the composite instruction */
   CompositeInstructionOrder order_{ CompositeInstructionOrder::ORDERED };
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/constants.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/constants.h
@@ -0,0 +1,33 @@
+/**
+ * @file constants.h
+ * @brief Containst Tesseract Command Language constants
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_COMMAND_LANGUAGE_CONSTANTS_H
+#define TESSERACT_COMMAND_LANGUAGE_CONSTANTS_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <string>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+namespace tesseract_planning
+{
+/** @brief Set to DEFAULT. Default profiles are given this name. */
+static const std::string DEFAULT_PROFILE_KEY = "DEFAULT";
+
+}  // namespace tesseract_planning
+#endif  // TESSERACT_COMMAND_LANGUAGE_CONSTANTS_H
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/move_instruction.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/move_instruction.h
@@ -33,6 +33,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/core/waypoint.h>
+#include <tesseract_command_language/constants.h>
 #include <tesseract_command_language/instruction_type.h>
 #include <tesseract_command_language/manipulator_info.h>
 
@@ -54,7 +55,7 @@ class MoveInstruction
 
   MoveInstruction(Waypoint waypoint,
                   MoveInstructionType type,
-                  const std::string& profile = "DEFAULT",
+                  const std::string& profile = DEFAULT_PROFILE_KEY,
                   ManipulatorInfo manipulator_info = ManipulatorInfo());
 
   void setWaypoint(Waypoint waypoint);
@@ -97,7 +98,7 @@ class MoveInstruction
   std::string description_;
 
   /** @brief The profile used for this move instruction */
-  std::string profile_{ "DEFAULT" };
+  std::string profile_{ DEFAULT_PROFILE_KEY };
 
   /** @brief The assigned waypoint (Cartesian or Joint) */
   Waypoint waypoint_;
diff --git a/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/plan_instruction.h b/tesseract/tesseract_planning/tesseract_command_language/include/tesseract_command_language/plan_instruction.h
@@ -34,6 +34,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_command_language/core/waypoint.h>
+#include <tesseract_command_language/constants.h>
 #include <tesseract_command_language/instruction_type.h>
 #include <tesseract_command_language/manipulator_info.h>
 
@@ -55,7 +56,7 @@ class PlanInstruction
 
   PlanInstruction(Waypoint waypoint,
                   PlanInstructionType type,
-                  std::string profile = "DEFAULT",
+                  std::string profile = DEFAULT_PROFILE_KEY,
                   ManipulatorInfo manipulator_info = ManipulatorInfo());
 
   void setWaypoint(Waypoint waypoint);
@@ -100,7 +101,7 @@ class PlanInstruction
   Waypoint waypoint_;
 
   /** @brief The profile used for this plan instruction */
-  std::string profile_{ "DEFAULT" };
+  std::string profile_{ DEFAULT_PROFILE_KEY };
 
   /** @brief Contains information about the manipulator associated with this instruction*/
   ManipulatorInfo manipulator_info_;
diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/composite_instruction.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/composite_instruction.cpp
@@ -52,7 +52,7 @@ void CompositeInstruction::setDescription(const std::string& description) { desc
 
 void CompositeInstruction::setProfile(const std::string& profile)
 {
-  profile_ = (profile.empty()) ? "DEFAULT" : profile;
+  profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
 }
 const std::string& CompositeInstruction::getProfile() const { return profile_; }
 
diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/move_instruction.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/move_instruction.cpp
@@ -58,7 +58,10 @@ void MoveInstruction::setManipulatorInfo(ManipulatorInfo info) { manipulator_inf
 const ManipulatorInfo& MoveInstruction::getManipulatorInfo() const { return manipulator_info_; }
 ManipulatorInfo& MoveInstruction::getManipulatorInfo() { return manipulator_info_; }
 
-void MoveInstruction::setProfile(const std::string& profile) { profile_ = (profile.empty()) ? "DEFAULT" : profile; }
+void MoveInstruction::setProfile(const std::string& profile)
+{
+  profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
+}
 const std::string& MoveInstruction::getProfile() const { return profile_; }
 
 int MoveInstruction::getType() const { return type_; }
diff --git a/tesseract/tesseract_planning/tesseract_command_language/src/plan_instruction.cpp b/tesseract/tesseract_planning/tesseract_command_language/src/plan_instruction.cpp
@@ -49,7 +49,10 @@ void PlanInstruction::setManipulatorInfo(ManipulatorInfo info) { manipulator_inf
 const ManipulatorInfo& PlanInstruction::getManipulatorInfo() const { return manipulator_info_; }
 ManipulatorInfo& PlanInstruction::getManipulatorInfo() { return manipulator_info_; }
 
-void PlanInstruction::setProfile(const std::string& profile) { profile_ = (profile.empty()) ? "DEFAULT" : profile; }
+void PlanInstruction::setProfile(const std::string& profile)
+{
+  profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;
+}
 const std::string& PlanInstruction::getProfile() const { return profile_; }
 
 int PlanInstruction::getType() const { return type_; }
diff --git a/tesseract/tesseract_planning/tesseract_command_language/test/serialize_test.cpp b/tesseract/tesseract_planning/tesseract_command_language/test/serialize_test.cpp
@@ -101,7 +101,7 @@ CompositeInstruction getProgram()
     transition_from_start.setDescription("transition_from_start");
     transition_from_start.push_back(plan_f1);
 
-    CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED);
+    CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED);
     transitions.setDescription("transitions");
     transitions.push_back(transition_from_start);
     transitions.push_back(transition_from_end);
@@ -130,7 +130,7 @@ CompositeInstruction getProgram()
     transition_from_start.setDescription("transition_from_start");
     transition_from_start.push_back(plan_f1);
 
-    CompositeInstruction transitions("DEFAULT", CompositeInstructionOrder::UNORDERED);
+    CompositeInstruction transitions(DEFAULT_PROFILE_KEY, CompositeInstructionOrder::UNORDERED);
     transitions.setDescription("transitions");
     transitions.push_back(transition_from_start);
     transitions.push_back(transition_from_end);
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/impl/descartes_motion_planner.hpp
@@ -45,6 +45,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/descartes/descartes_motion_planner.h>
+#include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>
 #include <tesseract_motion_planners/core/utils.h>
 
 #include <tesseract_command_language/command_language.h>
@@ -56,6 +57,7 @@ template <typename FloatType>
 DescartesMotionPlanner<FloatType>::DescartesMotionPlanner(std::string name)
   : MotionPlanner(name), status_category_(std::make_shared<const DescartesMotionPlannerStatusCategory>(name))
 {
+  plan_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<DescartesDefaultPlanProfile<FloatType>>();
 }
 
 template <typename FloatType>
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/descartes/problem_generators/default_problem_generator.h
@@ -28,6 +28,7 @@
 
 #include <tesseract_motion_planners/core/utils.h>
 #include <tesseract_motion_planners/descartes/descartes_problem.h>
+#include <tesseract_motion_planners/planner_utils.h>
 #include <tesseract_motion_planners/descartes/profile/descartes_profile.h>
 #include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>
 #include <tesseract_kinematics/core/validate.h>
@@ -119,25 +120,11 @@ DefaultDescartesProblemGenerator(const std::string& name,
     start_waypoint = swp;
   }
 
-  // Check Start Profile
-  if (profile.empty())
-    profile = "DEFAULT";
-
-  // Check for remapping of profile
-  auto remap = request.plan_profile_remapping.find(name);
-  if (remap != request.plan_profile_remapping.end())
-  {
-    auto p = remap->second.find(profile);
-    if (p != remap->second.end())
-      profile = p->second;
-  }
-
-  typename DescartesPlanProfile<FloatType>::Ptr cur_plan_profile{ nullptr };
-  auto it = plan_profiles.find(profile);
-  if (it == plan_profiles.end())
-    cur_plan_profile = std::make_shared<DescartesDefaultPlanProfile<FloatType>>();
-  else
-    cur_plan_profile = it->second;
+  profile = getProfileString(profile, name, request.plan_profile_remapping);
+  auto cur_plan_profile = getProfile<DescartesPlanProfile<FloatType>>(
+      profile, plan_profiles, std::make_shared<DescartesDefaultPlanProfile<FloatType>>());
+  if (!cur_plan_profile)
+    throw std::runtime_error("DescartesMotionPlannerConfig: Invalid profile");
 
   // Add start waypoint
   if (isCartesianWaypoint(start_waypoint))
@@ -180,24 +167,11 @@ DefaultDescartesProblemGenerator(const std::string& name,
 
       // Get Plan Profile
       std::string profile = plan_instruction->getProfile();
-      if (profile.empty())
-        profile = "DEFAULT";
-
-      // Check for remapping of profile
-      auto remap = request.plan_profile_remapping.find(name);
-      if (remap != request.plan_profile_remapping.end())
-      {
-        auto p = remap->second.find(profile);
-        if (p != remap->second.end())
-          profile = p->second;
-      }
-
-      typename DescartesPlanProfile<FloatType>::Ptr cur_plan_profile{ nullptr };
-      auto it = plan_profiles.find(profile);
-      if (it == plan_profiles.end())
-        cur_plan_profile = std::make_shared<DescartesDefaultPlanProfile<FloatType>>();
-      else
-        cur_plan_profile = it->second;
+      profile = getProfileString(profile, name, request.plan_profile_remapping);
+      auto cur_plan_profile = getProfile<DescartesPlanProfile<FloatType>>(
+          profile, plan_profiles, std::make_shared<DescartesDefaultPlanProfile<FloatType>>());
+      if (!cur_plan_profile)
+        throw std::runtime_error("DescartesMotionPlannerConfig: Invalid profile");
 
       if (plan_instruction->isLinear())
       {
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/planner_utils.h b/tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/planner_utils.h
@@ -29,7 +29,10 @@
 #include <tesseract_common/macros.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
+#include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
+#include <tesseract_motion_planners/robot_config.h>
+#include <tesseract_motion_planners/core/types.h>
 
 namespace tesseract_planning
 {
@@ -101,6 +104,63 @@ bool isValidState(const tesseract_kinematics::ForwardKinematics::ConstPtr& robot
   return !(robot_config != RobotConfig::FUT && robot_config != RobotConfig::NUT);
 }
 
+/**
+ * @brief Get the profile string taking into account defaults and profile remapping
+ * @param profile The requested profile name in the instructions
+ * @param planner_name Used to look up if there are remappings available
+ * @param profile_remapping Remapping used to remap a profile name based on the planner name
+ * @param default_profile Default = DEFAULT. This is set if profile.empty()
+ * @return The profile string taking into account defaults and profile remapping
+ */
+inline std::string getProfileString(const std::string& profile,
+                                    const std::string& planner_name,
+                                    const PlannerProfileRemapping& profile_remapping,
+                                    std::string default_profile = "DEFAULT")
+{
+  std::string results = profile;
+  if (profile.empty())
+    results = std::move(default_profile);
+
+  // Check for remapping of profile
+  auto remap = profile_remapping.find(planner_name);
+  if (remap != profile_remapping.end())
+  {
+    auto p = remap->second.find(profile);
+    if (p != remap->second.end())
+      results = p->second;
+  }
+  return results;
+}
+
+/**
+ * @brief Gets the profile specified from the profile map
+ * @param profile The requested profile
+ * @param profile_map map that contains the profiles
+ * @param default_profile Profile that is returned if the requested profile is not found in the map. Default = nullptr
+ * @return The profile requested if found. Otherwise the default_profile
+ */
+template <typename ProfileType>
+std::shared_ptr<ProfileType>
+getProfile(const std::string& profile,
+           const std::unordered_map<std::string, std::shared_ptr<ProfileType>>& profile_map,
+           std::shared_ptr<ProfileType> default_profile = nullptr)
+{
+  std::shared_ptr<ProfileType> results;
+  auto it = profile_map.find(profile);
+
+  if (it == profile_map.end())
+  {
+    CONSOLE_BRIDGE_logDebug("Profile %s was not found. Using default if available. Available profiles:",
+                            profile.c_str());
+    for (const auto& pair : profile_map)
+      CONSOLE_BRIDGE_logDebug("%s", pair.first.c_str());
+    results = default_profile;
+  }
+  else
+    results = it->second;
+
+  return results;
+}
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_MOTION_PLANNERS_PLANNER_UTILS_H
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/core/planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/core/planner.cpp
@@ -0,0 +1,26 @@
+/**
+ * @file planner.cpp
+ * @brief
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <tesseract_motion_planners/core/planner.h>
+
+namespace tesseract_planning
+{
+const std::string MotionPlanner::DEFAULT_PROFILE_KEY = "DEFAULT";
+
+};  // namespace tesseract_planning
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/ompl_motion_planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/ompl_motion_planner.cpp
@@ -39,6 +39,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_motion_planners/ompl/ompl_motion_planner.h>
 #include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
 #include <tesseract_motion_planners/ompl/discrete_motion_validator.h>
+#include <tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h>
 #include <tesseract_motion_planners/ompl/weighted_real_vector_state_sampler.h>
 #include <tesseract_motion_planners/core/utils.h>
 
@@ -89,6 +90,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
 OMPLMotionPlanner::OMPLMotionPlanner(std::string name)
   : MotionPlanner(std::move(name)), status_category_(std::make_shared<const OMPLMotionPlannerStatusCategory>(name_))
 {
+  plan_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<OMPLDefaultPlanProfile>();
 }
 
 bool OMPLMotionPlanner::terminate()
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/ompl/problem_generators/default_problem_generator.cpp
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/simple/simple_motion_planner.cpp
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/problem_generators/default_problem_generator.cpp
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/src/trajopt/trajopt_motion_planner.cpp
diff --git a/tesseract/tesseract_planning/tesseract_motion_planners/test/utils_test.cpp b/tesseract/tesseract_planning/tesseract_motion_planners/test/utils_test.cpp
diff --git a/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/iterative_spline_parameterization_process_generator.cpp b/tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/iterative_spline_parameterization_process_generator.cpp

Original file line number	Diff line number	Diff line change
`@@ -52,7 +52,7 @@ void CompositeInstruction::setDescription(const std::string& description) { desc`
`52`	`52`
`53`	`53`	`void CompositeInstruction::setProfile(const std::string& profile)`
`54`	`54`	`{`
`55`		`- profile_ = (profile.empty()) ? "DEFAULT" : profile;`
	`55`	`+ profile_ = (profile.empty()) ? DEFAULT_PROFILE_KEY : profile;`
`56`	`56`	`}`
`57`	`57`	`const std::string& CompositeInstruction::getProfile() const { return profile_; }`
`58`	`58`
Original file line number	Diff line number	Diff line change
`@@ -45,6 +45,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH`
`45`	`45`	`TESSERACT_COMMON_IGNORE_WARNINGS_POP`
`46`	`46`
`47`	`47`	`#include <tesseract_motion_planners/descartes/descartes_motion_planner.h>`
	`48`	`+#include <tesseract_motion_planners/descartes/profile/descartes_default_plan_profile.h>`
`48`	`49`	`#include <tesseract_motion_planners/core/utils.h>`
`49`	`50`
`50`	`51`	`#include <tesseract_command_language/command_language.h>`
`@@ -56,6 +57,7 @@ template <typename FloatType>`
`56`	`57`	`DescartesMotionPlanner<FloatType>::DescartesMotionPlanner(std::string name)`
`57`	`58`	`: MotionPlanner(name), status_category_(std::make_shared<const DescartesMotionPlannerStatusCategory>(name))`
`58`	`59`	`{`
	`60`	`+ plan_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<DescartesDefaultPlanProfile<FloatType>>();`
`59`	`61`	`}`
`60`	`62`
`61`	`63`	`template <typename FloatType>`