Move ManipulatorManager into Environment

Author: mpowelson

tesseract/tesseract/CMakeLists.txt

@@ -36,15 +36,13 @@ add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE})
 tesseract_variables()
 
 # Create target
-add_library(${PROJECT_NAME} src/tesseract.cpp src/manipulator_manager.cpp)
+add_library(${PROJECT_NAME} src/tesseract.cpp)
 target_link_libraries(${PROJECT_NAME} PUBLIC
   tesseract::tesseract_urdf
   tesseract::tesseract_scene_graph
   tesseract::tesseract_collision_bullet
   tesseract::tesseract_environment_kdl
   tesseract::tesseract_environment_ofkt
-  tesseract::tesseract_kinematics_kdl
-  tesseract::tesseract_kinematics_opw
   tesseract::tesseract_command_language
   console_bridge::console_bridge)
 target_compile_options(${PROJECT_NAME} PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})

tesseract/tesseract/include/tesseract/tesseract.h

@@ -37,10 +37,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract/tesseract_init_info.h>
 #include <tesseract_environment/core/environment.h>
-#include <tesseract_scene_graph/srdf_model.h>
 #include <tesseract_scene_graph/utils.h>
 #include <tesseract_scene_graph/resource_locator.h>
-#include <tesseract/manipulator_manager.h>
 #include <tesseract_command_language/manipulator_info.h>
 
 namespace tesseract
@@ -74,6 +72,7 @@ class Tesseract
 
   bool isInitialized() const;
 
+  bool init(const tesseract_environment::Environment& env);
   bool init(tesseract_scene_graph::SceneGraph::Ptr scene_graph);
   bool init(tesseract_scene_graph::SceneGraph::Ptr scene_graph, tesseract_scene_graph::SRDFModel::Ptr srdf_model);
   bool init(const std::string& urdf_string, const tesseract_scene_graph::ResourceLocator::Ptr& locator);
@@ -85,8 +84,6 @@ class Tesseract
             const boost::filesystem::path& srdf_path,
             const tesseract_scene_graph::ResourceLocator::Ptr& locator);
 
-  bool init(const tesseract_environment::Environment& env, const ManipulatorManager& manipulator_manager);
-
   bool init(const TesseractInitInfo::Ptr& init_info);
 
   /** @brief Clone the Tesseract and all of the internals
@@ -102,8 +99,10 @@ class Tesseract
   tesseract_environment::Environment::Ptr getEnvironment();
   tesseract_environment::Environment::ConstPtr getEnvironment() const;
 
-  ManipulatorManager::Ptr getManipulatorManager();
-  ManipulatorManager::ConstPtr getManipulatorManager() const;
+  DEPRECATED("Please use getEnvironment->getManipulatorManager()")
+  tesseract_environment::ManipulatorManager::Ptr getManipulatorManager();
+  DEPRECATED("Please use getEnvironment->getManipulatorManager()")
+  tesseract_environment::ManipulatorManager::ConstPtr getManipulatorManager() const;
 
   /**
    * @brief Find tool center point provided in the manipulator info
@@ -132,7 +131,6 @@ class Tesseract
 private:
   bool initialized_;
   tesseract_environment::Environment::Ptr environment_;
-  ManipulatorManager::Ptr manipulator_manager_;
   TesseractInitInfo::Ptr init_info_;
   std::vector<FindTCPCallbackFn> find_tcp_cb_;
 

tesseract/tesseract/include/tesseract/tesseract_init_info.h

@@ -31,7 +31,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <boost/filesystem.hpp>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract/manipulator_manager.h>
 #include <tesseract_environment/core/environment.h>
 #include <tesseract_scene_graph/resource_locator.h>
 #include <tesseract_scene_graph/graph.h>
@@ -48,7 +47,7 @@ enum class TesseractInitType
   URDF_STRING_SRDF_STRING,
   URDF_PATH,
   URDF_PATH_SRDF_PATH,
-  ENVIRONMENT_MANIPULATOR_MANAGER
+  ENVIRONMENT
 };
 
 /** @brief Used to store information about how a given Tesseract was initialized. See the Tesseract init methods.
@@ -79,19 +78,12 @@ struct TesseractInitInfo
   tesseract_scene_graph::SRDFModel::Ptr srdf_model;
 
   /**
-   * @brief Used when InitType is ENVIRONMENT_MANIPULATOR_MANAGER
+   * @brief Used when InitType is ENVIRONMENT
    *
    * This store a clone of the environment
    */
   tesseract_environment::Environment::Ptr environment;
 
-  /**
-   * @brief Used when InitType is ENVIRONMENT_MANIPULATOR_MANAGER
-   *
-   * This stores a clone of the manipulator manager
-   */
-  tesseract::ManipulatorManager::Ptr manipulator_manager;
-
   /** @brief Used when InitType is URDF_STRING and URDF_STRING_SRDF_STRING*/
   std::string urdf_string;
   /** @brief Used when InitType is URDF_STRING_SRDF_STRING*/

tesseract/tesseract/src/tesseract.cpp

@@ -66,13 +66,6 @@ bool Tesseract::init(tesseract_scene_graph::SceneGraph::Ptr scene_graph)
   auto srdf = std::make_shared<tesseract_scene_graph::SRDFModel>();
   srdf->getName() = scene_graph->getName();
 
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, srdf))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
@@ -92,19 +85,12 @@ bool Tesseract::init(tesseract_scene_graph::SceneGraph::Ptr scene_graph,
 
   // Construct Environment from Scene Graph
   environment_ = std::make_shared<tesseract_environment::Environment>();
-  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph))
+  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph, srdf_model))
   {
     CONSOLE_BRIDGE_logError("Failed to initialize environment.");
     return false;
   }
 
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, std::make_shared<tesseract_scene_graph::SRDFModel>(*srdf_model)))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
@@ -135,16 +121,6 @@ bool Tesseract::init(const std::string& urdf_string, const tesseract_scene_graph
     return false;
   }
 
-  auto srdf = std::make_shared<tesseract_scene_graph::SRDFModel>();
-  srdf->getName() = scene_graph->getName();
-
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, srdf))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
@@ -183,19 +159,12 @@ bool Tesseract::init(const std::string& urdf_string,
 
   // Construct Environment
   environment_ = std::make_shared<tesseract_environment::Environment>();
-  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph))
+  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph, srdf))
   {
     CONSOLE_BRIDGE_logError("Failed to initialize environment.");
     return false;
   }
 
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, srdf))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
@@ -227,16 +196,6 @@ bool Tesseract::init(const boost::filesystem::path& urdf_path,
     return false;
   }
 
-  auto srdf = std::make_shared<tesseract_scene_graph::SRDFModel>();
-  srdf->getName() = scene_graph->getName();
-
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, srdf))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
@@ -275,34 +234,24 @@ bool Tesseract::init(const boost::filesystem::path& urdf_path,
 
   // Construct Environment
   environment_ = std::make_shared<tesseract_environment::Environment>();
-  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph))
+  if (!environment_->init<tesseract_environment::OFKTStateSolver>(*scene_graph, srdf))
   {
     CONSOLE_BRIDGE_logError("Failed to initialize environment.");
     return false;
   }
 
-  manipulator_manager_ = std::make_shared<ManipulatorManager>();
-  if (!manipulator_manager_->init(environment_, srdf))
-  {
-    CONSOLE_BRIDGE_logError("Failed to initialize manipulator manager.");
-    return false;
-  }
-
   registerDefaultContactManagers();
 
   initialized_ = true;
   return true;
 }
 
-bool Tesseract::init(const tesseract_environment::Environment& env, const ManipulatorManager& manipulator_manager)
+bool Tesseract::init(const tesseract_environment::Environment& env)
 {
   clear();
   init_info_ = std::make_shared<TesseractInitInfo>();
-  init_info_->type = TesseractInitType::ENVIRONMENT_MANIPULATOR_MANAGER;
+  init_info_->type = TesseractInitType::ENVIRONMENT;
   init_info_->environment = env.clone();
-  init_info_->manipulator_manager = manipulator_manager.clone(init_info_->environment);
-  environment_ = env.clone();
-  manipulator_manager_ = manipulator_manager.clone(environment_);
   initialized_ = true;
   return initialized_;
 }
@@ -329,8 +278,8 @@ bool Tesseract::init(const TesseractInitInfo::Ptr& init_info)
     case TesseractInitType::URDF_PATH_SRDF_PATH:
       init(init_info->urdf_path, init_info->srdf_path, init_info->resource_locator);
       break;
-    case TesseractInitType::ENVIRONMENT_MANIPULATOR_MANAGER:
-      init(*init_info->environment, *init_info->manipulator_manager);
+    case TesseractInitType::ENVIRONMENT:
+      init(*init_info->environment);
       break;
     default:
       CONSOLE_BRIDGE_logError("Unsupported TesseractInitInfo type.");
@@ -346,10 +295,6 @@ Tesseract::Ptr Tesseract::clone() const
   if (environment_)
     clone->environment_ = environment_->clone();
 
-  if (clone->environment_)
-    if (manipulator_manager_)
-      clone->manipulator_manager_ = manipulator_manager_->clone(clone->environment_);
-
   clone->init_info_ = init_info_;
   clone->initialized_ = initialized_;
   clone->find_tcp_cb_ = find_tcp_cb_;
@@ -372,8 +317,14 @@ const tesseract_scene_graph::ResourceLocator::Ptr& Tesseract::getResourceLocator
 tesseract_environment::Environment::Ptr Tesseract::getEnvironment() { return environment_; }
 tesseract_environment::Environment::ConstPtr Tesseract::getEnvironment() const { return environment_; }
 
-ManipulatorManager::Ptr Tesseract::getManipulatorManager() { return manipulator_manager_; }
-ManipulatorManager::ConstPtr Tesseract::getManipulatorManager() const { return manipulator_manager_; }
+tesseract_environment::ManipulatorManager::Ptr Tesseract::getManipulatorManager()
+{
+  return environment_->getManipulatorManager();
+}
+tesseract_environment::ManipulatorManager::ConstPtr Tesseract::getManipulatorManager() const
+{
+  return environment_->getManipulatorManager();
+}
 
 /** @brief registerDefaultContactManagers */
 bool Tesseract::registerDefaultContactManagers()
@@ -399,7 +350,6 @@ void Tesseract::clear()
 {
   initialized_ = false;
   environment_ = nullptr;
-  manipulator_manager_ = nullptr;
   init_info_ = nullptr;
   find_tcp_cb_.clear();
 }
@@ -409,7 +359,7 @@ Eigen::Isometry3d Tesseract::findTCP(const tesseract_planning::ManipulatorInfo&
   if (manip_info.tcp.empty())
     return Eigen::Isometry3d::Identity();
 
-  auto composite_mi_fwd_kin = manipulator_manager_->getFwdKinematicSolver(manip_info.manipulator);
+  auto composite_mi_fwd_kin = environment_->getManipulatorManager()->getFwdKinematicSolver(manip_info.manipulator);
   if (composite_mi_fwd_kin == nullptr)
     throw std::runtime_error("findTCP: Manipulator '" + manip_info.manipulator + "' does not exist!");
 
@@ -418,8 +368,8 @@ Eigen::Isometry3d Tesseract::findTCP(const tesseract_planning::ManipulatorInfo&
   {
     // Check Manipulator Manager for TCP
     const std::string& tcp_name = manip_info.tcp.getString();
-    if (manipulator_manager_->hasGroupTCP(manip_info.manipulator, tcp_name))
-      return manipulator_manager_->getGroupsTCP(manip_info.manipulator, tcp_name);
+    if (environment_->getManipulatorManager()->hasGroupTCP(manip_info.manipulator, tcp_name))
+      return environment_->getManipulatorManager()->getGroupsTCP(manip_info.manipulator, tcp_name);
 
     // Check Environment for links and calculate TCP
     tesseract_environment::EnvState::ConstPtr env_state = environment_->getCurrentState();

tesseract/tesseract/test/tesseract_unit.cpp

@@ -84,10 +84,6 @@ TEST(TesseractUnit, TesseractURDFPathSRDFPathLocatorInitUnit)  // NOLINT
 {
 }
 
-TEST(TesseractUnit, TesseractEnvironmentManipulatorManagerInitUnit)  // NOLINT
-{
-}
-
 TEST(TesseractUnit, TesseractInitInfoInitUnit)  // NOLINT
 {
 }

tesseract/tesseract_environment/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(Eigen3 REQUIRED)
 find_package(orocos_kdl REQUIRED)
 find_package(console_bridge REQUIRED)
 find_package(tesseract_collision REQUIRED)
+find_package(tesseract_kinematics REQUIRED)
 find_package(tesseract_scene_graph REQUIRED)
 find_package(tesseract_common REQUIRED)
 find_package(cmake_common_scripts REQUIRED)
@@ -33,8 +34,13 @@ add_code_coverage_all_targets(EXCLUDE ${COVERAGE_EXCLUDE})
 tesseract_variables()
 
 # Create interface for core
-add_library(${PROJECT_NAME}_core src/core/environment.cpp)
-target_link_libraries(${PROJECT_NAME}_core PUBLIC tesseract::tesseract_common tesseract::tesseract_collision_core tesseract::tesseract_scene_graph)
+add_library(${PROJECT_NAME}_core src/core/environment.cpp src/manipulator_manager/manipulator_manager.cpp)
+target_link_libraries(${PROJECT_NAME}_core PUBLIC
+  tesseract::tesseract_common
+  tesseract::tesseract_collision_core
+  tesseract::tesseract_scene_graph
+  tesseract::tesseract_kinematics_kdl
+  tesseract::tesseract_kinematics_opw)
 target_compile_options(${PROJECT_NAME}_core PRIVATE ${TESSERACT_COMPILE_OPTIONS_PRIVATE})
 target_compile_options(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_OPTIONS_PUBLIC})
 target_compile_definitions(${PROJECT_NAME}_core PUBLIC ${TESSERACT_COMPILE_DEFINITIONS})
@@ -76,7 +82,7 @@ target_include_directories(${PROJECT_NAME}_ofkt PUBLIC
 target_include_directories(${PROJECT_NAME}_ofkt SYSTEM PUBLIC
     ${EIGEN3_INCLUDE_DIRS})
 
-configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME}_core ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ofkt)
+  configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME}_core ${PROJECT_NAME}_kdl ${PROJECT_NAME}_ofkt)
 
 # Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}

tesseract/tesseract_environment/include/tesseract_environment/core/commands.h

@@ -288,13 +288,11 @@ class AddSceneGraphCommand : public Command
   std::string prefix_;
 };
 
-class TESSERACT_ENVIRONMENT_CORE_PUBLIC ChangeJointLimitsCommand : public Command
+class ChangeJointLimitsCommand : public Command
 {
 public:
   ChangeJointLimitsCommand(std::string joint_name, tesseract_scene_graph::JointLimits joint_limits)
-    : Command(CommandType::CHANGE_JOINT_LIMITS)
-    , joint_name_(std::move(joint_name))
-    , joint_limits_(std::move(joint_limits))
+    : Command(CommandType::CHANGE_JOINT_LIMITS), joint_name_(std::move(joint_name)), joint_limits_(joint_limits)
   {
   }
   const std::string& getJointName() const { return joint_name_; }