Add ProfileSwitchProcessGenerator

This generator simply returns a value specified in the composite profile. This can be used to switch execution based on the profile

Author: mpowelson

tesseract/tesseract_planning/tesseract_motion_planners/include/tesseract_motion_planners/planner_utils.h

@@ -31,6 +31,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 #include <console_bridge/console.h>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_command_language/constants.h>
+#include <tesseract_kinematics/core/forward_kinematics.h>
 #include <tesseract_motion_planners/robot_config.h>
 #include <tesseract_motion_planners/core/types.h>
 
@@ -115,7 +118,7 @@ bool isValidState(const tesseract_kinematics::ForwardKinematics::ConstPtr& robot
 inline std::string getProfileString(const std::string& profile,
                                     const std::string& planner_name,
                                     const PlannerProfileRemapping& profile_remapping,
-                                    std::string default_profile = "DEFAULT")
+                                    std::string default_profile = DEFAULT_PROFILE_KEY)
 {
   std::string results = profile;
   if (profile.empty())

tesseract/tesseract_planning/tesseract_process_managers/CMakeLists.txt

@@ -46,6 +46,7 @@ add_library(${PROJECT_NAME}
     src/process_generators/fix_state_collision_process_generator.cpp
     src/process_generators/iterative_spline_parameterization_process_generator.cpp
     src/process_generators/motion_planner_process_generator.cpp
+    src/process_generators/profile_switch_process_generator.cpp
     src/process_managers/raster_process_manager.cpp
     src/process_managers/raster_dt_process_manager.cpp
     src/process_managers/raster_waad_process_manager.cpp

tesseract/tesseract_planning/tesseract_process_managers/include/tesseract_process_managers/process_generators/profile_switch_process_generator.h

@@ -0,0 +1,87 @@
+/**
+ * @file profile_switch_process_generator.h
+ * @brief Process generator that returns a value based on the profile
+ *
+ * @author Matthew Powelson
+ * @date October 26. 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @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_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H
+#define TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <atomic>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_process_managers/process_generator.h>
+
+namespace tesseract_planning
+{
+struct ProfileSwitchProfile
+{
+  ProfileSwitchProfile(const int& return_value = 1);
+
+  using Ptr = std::shared_ptr<ProfileSwitchProfile>;
+  using ConstPtr = std::shared_ptr<const ProfileSwitchProfile>;
+
+  int return_value;
+};
+using ProfileSwitchProfileMap = std::unordered_map<std::string, ProfileSwitchProfile::Ptr>;
+
+/**
+ * @brief This generator simply returns a value specified in the composite profile. This can be used to switch execution
+ * based on the profile
+ */
+class ProfileSwitchProcessGenerator : public ProcessGenerator
+{
+public:
+  using UPtr = std::unique_ptr<ProfileSwitchProcessGenerator>;
+
+  ProfileSwitchProcessGenerator(std::string name = "Profile Switch");
+
+  ~ProfileSwitchProcessGenerator() override = default;
+  ProfileSwitchProcessGenerator(const ProfileSwitchProcessGenerator&) = delete;
+  ProfileSwitchProcessGenerator& operator=(const ProfileSwitchProcessGenerator&) = delete;
+  ProfileSwitchProcessGenerator(ProfileSwitchProcessGenerator&&) = delete;
+  ProfileSwitchProcessGenerator& operator=(ProfileSwitchProcessGenerator&&) = delete;
+
+  const std::string& getName() const override;
+
+  std::function<void()> generateTask(ProcessInput input) override;
+
+  std::function<int()> generateConditionalTask(ProcessInput input) override;
+
+  bool getAbort() const override;
+
+  void setAbort(bool abort) override;
+
+  ProfileSwitchProfileMap composite_profiles;
+
+private:
+  /** @brief If true, all tasks return immediately. Workaround for https://github.com/taskflow/taskflow/issues/201 */
+  std::atomic<bool> abort_{ false };
+
+  std::string name_;
+
+  int conditionalProcess(ProcessInput input) const;
+
+  void process(ProcessInput input) const;
+};
+}  // namespace tesseract_planning
+#endif  // TESSERACT_PROCESS_MANAGERS_PROFILE_SWITCH_PROCESS_GENERATOR_H

tesseract/tesseract_planning/tesseract_process_managers/src/process_generators/profile_switch_process_generator.cpp

@@ -0,0 +1,94 @@
+/**
+ * @file profile_switch_process_generator.h
+ * @brief Process generator that returns a value based on the profile
+ *
+ * @author Matthew Powelson
+ * @date October 26. 2020
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @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_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <console_bridge/console.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_process_managers/process_generators/profile_switch_process_generator.h>
+#include <tesseract_command_language/constants.h>
+#include <tesseract_command_language/utils/utils.h>
+#include <tesseract_motion_planners/planner_utils.h>
+
+namespace tesseract_planning
+{
+ProfileSwitchProfile::ProfileSwitchProfile(const int& return_value) : return_value(return_value) {}
+
+ProfileSwitchProcessGenerator::ProfileSwitchProcessGenerator(std::string name) : name_(std::move(name))
+{
+  // Register default profile
+  composite_profiles[DEFAULT_PROFILE_KEY] = std::make_shared<ProfileSwitchProfile>();
+}
+
+const std::string& ProfileSwitchProcessGenerator::getName() const { return name_; }
+
+std::function<void()> ProfileSwitchProcessGenerator::generateTask(ProcessInput input)
+{
+  return [=]() { process(input); };
+}
+
+std::function<int()> ProfileSwitchProcessGenerator::generateConditionalTask(ProcessInput input)
+{
+  return [=]() { return conditionalProcess(input); };
+}
+
+int ProfileSwitchProcessGenerator::conditionalProcess(ProcessInput input) const
+{
+  if (abort_)
+    return 0;
+
+  // --------------------
+  // Check that inputs are valid
+  // --------------------
+  const Instruction* input_instruction = input.getInstruction();
+  if (!isCompositeInstruction(*input_instruction))
+  {
+    CONSOLE_BRIDGE_logError("Input instruction to ProfileSwitch must be a composite instruction. Returning 0");
+    return 0;
+  }
+
+  const auto* ci = input_instruction->cast_const<CompositeInstruction>();
+
+  // Get the profile
+  std::string profile = getProfileString(ci->getProfile(), name_, input.composite_profile_remapping);
+  ProfileSwitchProfile::Ptr cur_composite_profile =
+      getProfile<ProfileSwitchProfile>(profile, composite_profiles, std::make_shared<ProfileSwitchProfile>());
+  if (!cur_composite_profile)
+  {
+    CONSOLE_BRIDGE_logWarn("ProfileSwitchProfile invalid. Returning 1");
+    return 1;
+  }
+
+  // Return the value specified in the profile
+  CONSOLE_BRIDGE_logDebug("ProfileSwitchProfile returning %d", cur_composite_profile->return_value);
+  return cur_composite_profile->return_value;
+}
+
+void ProfileSwitchProcessGenerator::process(ProcessInput input) const { conditionalProcess(input); }
+
+bool ProfileSwitchProcessGenerator::getAbort() const { return abort_; }
+void ProfileSwitchProcessGenerator::setAbort(bool abort) { abort_ = abort; }
+
+}  // namespace tesseract_planning