Update due to changes with profiles and overrides

tesseract_msgs/CMakeLists.txt

@@ -65,7 +65,6 @@ set(message_files
   "msg/Mesh.msg"
   "msg/ObjectColor.msg"
   "msg/OctreeSubType.msg"
-  "msg/PlannerProfileRemapping.msg"
   "msg/PlanningRequest.msg"
   "msg/PlanningResponse.msg"
   "msg/PluginInfo.msg"

tesseract_msgs/msg/PlanningRequest.msg

@@ -7,6 +7,3 @@ string input # This should be an xml string of tesseract_common::AnyPoly
 bool dotgraph # Enable if DOT Graph should be generated and returned
 bool debug # Enable debug content
 bool save_io # Enable saving of input and output for task infos
-
-tesseract_msgs/PlannerProfileRemapping move_profile_remapping
-tesseract_msgs/PlannerProfileRemapping composite_profile_remapping

tesseract_planning_server/src/tesseract_planning_server.cpp

@@ -251,13 +251,6 @@ void TesseractPlanningServer::onMotionPlanningCallback(
   data->setData("planning_input", std::move(planning_input));
   data->setData("environment", std::shared_ptr<const tesseract_environment::Environment>(std::move(env)));
   data->setData("profiles", profiles_);
-  auto move_profile_remapping = tesseract_rosutils::fromMsg(goal->request.move_profile_remapping);
-  if (!move_profile_remapping.empty())
-    data->setData("move_profile_remapping", move_profile_remapping);
-
-  auto composite_profile_remapping = tesseract_rosutils::fromMsg(goal->request.composite_profile_remapping);
-  if (!composite_profile_remapping.empty())
-    data->setData("composite_profile_remapping", composite_profile_remapping);
 
   // Solve
   tesseract_common::Stopwatch stopwatch;
@@ -301,69 +294,58 @@ void TesseractPlanningServer::onMotionPlanningCallback(
 void TesseractPlanningServer::loadDefaultPlannerProfiles()
 {
   // Add TrajOpt Default Profiles
-  profiles_->addProfile<tesseract_planning::TrajOptPlanProfile>(
-      TRAJOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptDefaultPlanProfile>());
-  profiles_->addProfile<tesseract_planning::TrajOptCompositeProfile>(
-      TRAJOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptDefaultCompositeProfile>());
-  profiles_->addProfile<tesseract_planning::TrajOptSolverProfile>(
-      TRAJOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptDefaultSolverProfile>());
+  profiles_->addProfile(TRAJOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptDefaultPlanProfile>());
+  profiles_->addProfile(TRAJOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptDefaultCompositeProfile>());
+  profiles_->addProfile(TRAJOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptDefaultSolverProfile>());
 
   // Add TrajOpt IFOPT Default Profiles
 #ifdef TESSERACT_TASK_COMPOSER_HAS_TRAJOPT_IFOPT
-  profiles_->addProfile<tesseract_planning::TrajOptIfoptPlanProfile>(
-      TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptIfoptDefaultPlanProfile>());
-  profiles_->addProfile<tesseract_planning::TrajOptIfoptCompositeProfile>(
-      TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptIfoptDefaultCompositeProfile>());
-  profiles_->addProfile<tesseract_planning::TrajOptIfoptSolverProfile>(
-      TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::TrajOptIfoptDefaultSolverProfile>());
+  profiles_->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptIfoptDefaultPlanProfile>());
+  profiles_->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptIfoptDefaultCompositeProfile>());
+  profiles_->addProfile(TRAJOPT_IFOPT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::TrajOptIfoptDefaultSolverProfile>());
 #endif
 
   // Add Descartes Default Profiles
-  profiles_->addProfile<tesseract_planning::DescartesPlanProfile<double>>(
-      DESCARTES_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::DescartesDefaultPlanProfile<double>>());
+  profiles_->addProfile(DESCARTES_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::DescartesDefaultPlanProfile<double>>());
 
   // Add OMPL Default Profiles
-  profiles_->addProfile<tesseract_planning::OMPLPlanProfile>(
-      OMPL_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::OMPLDefaultPlanProfile>());
+  profiles_->addProfile(OMPL_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::OMPLDefaultPlanProfile>());
 
   // Add Simple Default Profiles
-  profiles_->addProfile<tesseract_planning::SimplePlannerPlanProfile>(
-      SIMPLE_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::SimplePlannerLVSNoIKPlanProfile>());
+  profiles_->addProfile(SIMPLE_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::SimplePlannerLVSNoIKPlanProfile>());
 
   // MinLengthTask calls the SimpleMotionPlanner to generate a seed path with waypoints interpolated in joint space
-  profiles_->addProfile<tesseract_planning::MinLengthProfile>(MLT_DEFAULT_NAMESPACE,
-                                                              tesseract_planning::DEFAULT_PROFILE_KEY,
-                                                              std::make_shared<tesseract_planning::MinLengthProfile>());
+  profiles_->addProfile(MLT_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::MinLengthProfile>());
 
   // Post hoc collision checking
-  profiles_->addProfile<tesseract_planning::ContactCheckProfile>(
-      DCC_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::ContactCheckProfile>());
+  profiles_->addProfile(DCC_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::ContactCheckProfile>());
 
   // Time parameterization
-  profiles_->addProfile<tesseract_planning::IterativeSplineParameterizationProfile>(
-      ISP_DEFAULT_NAMESPACE,
-      tesseract_planning::DEFAULT_PROFILE_KEY,
-      std::make_shared<tesseract_planning::IterativeSplineParameterizationProfile>());
+  profiles_->addProfile(ISP_DEFAULT_NAMESPACE,
+                        tesseract_planning::DEFAULT_PROFILE_KEY,
+                        std::make_shared<tesseract_planning::IterativeSplineParameterizationProfile>());
 }
 
 Eigen::Isometry3d TesseractPlanningServer::tfFindTCPOffset(const tesseract_common::ManipulatorInfo& manip_info)

tesseract_rosutils/CMakeLists.txt

@@ -9,6 +9,7 @@ project(${pkg_extracted_name} VERSION ${pkg_extracted_version} LANGUAGES C CXX)
 find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(tf2_eigen REQUIRED)
+find_package(octomap_msgs REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(visualization_msgs REQUIRED)
 # tesseract
@@ -20,7 +21,6 @@ find_package(tesseract_msgs REQUIRED)
 
 # Load variable for clang tidy args, compiler options and cxx version
 tesseract_variables()
-
 add_library(${PROJECT_NAME} SHARED src/plotting.cpp src/conversions.cpp src/utils.cpp)
 target_link_libraries(${PROJECT_NAME}
   PUBLIC
@@ -34,6 +34,7 @@ target_link_libraries(${PROJECT_NAME}
   PRIVATE
     ament_index_cpp::ament_index_cpp
     ${tf2_eigen_TARGETS}
+    ${octomap_msgs_TARGETS}
 )
 target_compile_options(${PROJECT_NAME} PRIVATE ${TESSERACT_COMPILE_OPTIONS})
 target_clang_tidy(${PROJECT_NAME} ARGUMENTS ${TESSERACT_CLANG_TIDY_ARGS} ENABLE ${TESSERACT_ENABLE_CLANG_TIDY})
@@ -68,4 +69,4 @@ ament_package()
 if(TESSERACT_ENABLE_TESTING)
   enable_testing()
   add_subdirectory(test)
-endif()
+endif()

tesseract_rosutils/include/tesseract_rosutils/utils.h

@@ -58,7 +58,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_msgs/msg/string_double_pair.hpp>
 #include <tesseract_msgs/msg/transform_map.hpp>
 #include <tesseract_msgs/msg/visual_geometry.hpp>
-#include <tesseract_msgs/msg/planner_profile_remapping.hpp>
 #include <tesseract_msgs/msg/task_composer_key.hpp>
 #include <tesseract_msgs/msg/task_composer_node_info.hpp>
 #include <geometry_msgs/msg/pose.hpp>
@@ -189,11 +188,6 @@ bool fromMsg(std::shared_ptr<tesseract_scene_graph::JointSafety>& joint_safety,
 
 bool toMsg(tesseract_msgs::msg::Joint& joint_msg, const tesseract_scene_graph::Joint& joint);
 
-tesseract_planning::PlannerProfileRemapping
-fromMsg(const tesseract_msgs::msg::PlannerProfileRemapping& profile_remapping_msg);
-tesseract_msgs::msg::PlannerProfileRemapping
-toMsg(const tesseract_planning::PlannerProfileRemapping& profile_remapping);
-
 tesseract_common::PairsCollisionMarginData
 fromMsg(const std::vector<tesseract_msgs::msg::ContactMarginPair>& contact_margin_pairs_msg);
 std::vector<tesseract_msgs::msg::ContactMarginPair>

tesseract_rosutils/package.xml

@@ -15,6 +15,7 @@
   <depend>tf2_eigen</depend>
   <depend>trajectory_msgs</depend>
   <depend>visualization_msgs</depend>
+  <depend>octomap_msgs</depend>
 
   <depend>tesseract_task_composer</depend>
 

tesseract_rosutils/src/utils.cpp

@@ -1040,41 +1040,6 @@ tesseract_scene_graph::Joint fromMsg(const tesseract_msgs::msg::Joint& joint_msg
   return joint;
 }
 
-tesseract_planning::PlannerProfileRemapping
-fromMsg(const tesseract_msgs::msg::PlannerProfileRemapping& profile_remapping_msg)
-{
-  tesseract_planning::PlannerProfileRemapping profile_remapping;
-  for (std::size_t i = 0; i < profile_remapping_msg.planner.size(); ++i)
-  {
-    std::unordered_map<std::string, std::string> mapping;
-    for (const auto& pair : profile_remapping_msg.mapping[i].pairs)
-      mapping.emplace(pair.first, pair.second);
-
-    profile_remapping[profile_remapping_msg.planner[i]] = mapping;
-  }
-
-  return profile_remapping;
-}
-
-tesseract_msgs::msg::PlannerProfileRemapping toMsg(const tesseract_planning::PlannerProfileRemapping& profile_remapping)
-{
-  tesseract_msgs::msg::PlannerProfileRemapping profile_remapping_msg;
-  for (const auto& planner_remapping : profile_remapping)
-  {
-    profile_remapping_msg.planner.push_back(planner_remapping.first);
-    tesseract_msgs::msg::ProfileMap mapping;
-    for (const auto& planner_pair : planner_remapping.second)
-    {
-      tesseract_msgs::msg::StringPair p;
-      p.first = planner_pair.first;
-      p.second = planner_pair.second;
-      mapping.pairs.push_back(p);
-    }
-    profile_remapping_msg.mapping.push_back(mapping);
-  }
-  return profile_remapping_msg;
-}
-
 tesseract_common::PairsCollisionMarginData
 fromMsg(const std::vector<tesseract_msgs::msg::ContactMarginPair>& contact_margin_pairs_msg)
 {