Re-enable moveit_ros_warehouse for moveit_ros_planning_interface (#424)

* Remove warehouse_ros_mongo from moveit_ros_planning_interface test depends
moveit · Apr 16, 2021 · fc6e6fe · fc6e6fe
1 parent 25cbb0e
commit fc6e6fe
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 45 deletions.
diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt
@@ -9,7 +9,7 @@ find_package(ament_cmake REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
-# find_package(moveit_ros_warehouse REQUIRED)
+find_package(moveit_ros_warehouse REQUIRED)
 # find_package(moveit_ros_manipulation REQUIRED)
 find_package(moveit_ros_move_group REQUIRED)
 find_package(geometry_msgs REQUIRED)
@@ -52,7 +52,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_msgs
   moveit_core
   moveit_ros_planning
-#  moveit_ros_warehouse
+  moveit_ros_warehouse
 #  moveit_ros_manipulation
   moveit_ros_move_group
   tf2

diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt
@@ -4,11 +4,12 @@ add_library(${MOVEIT_LIB_NAME} SHARED src/move_group_interface.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects moveit_planning_scene_interface ${Boost_THREAD_LIBRARY})
 ament_target_dependencies(${MOVEIT_LIB_NAME}
-  moveit_ros_occupancy_map_monitor
+  moveit_core
+  moveit_msgs
   moveit_ros_move_group
+  moveit_ros_occupancy_map_monitor
   moveit_ros_planning
-  moveit_msgs
-  moveit_core
+  moveit_ros_warehouse
 )
 
 # TODO (ddengster) : port wrap_python_move_group

diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
@@ -39,8 +39,7 @@
 #include <stdexcept>
 #include <sstream>
 #include <memory>
-// TODO(JafarAbdi): Enable once moveit_ros_warehouse is ported
-// #include <moveit/warehouse/constraints_storage.h>
+#include <moveit/warehouse/constraints_storage.h>
 #include <moveit/kinematic_constraints/utils.h>
 #include <moveit/move_group/capability_names.h>
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -1193,22 +1192,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
 
   bool setPathConstraints(const std::string& constraint)
   {
-    // TODO(JafarAbdi): Enable once moveit_ros_warehouse is ported
-    //    if (constraints_storage_)
-    //    {
-    //
-    //      moveit_warehouse::ConstraintsWithMetadata msg_m;
-    //      if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
-    //      {
-    //        path_constraints_.reset(new
-    //        moveit_msgs::msg::Constraints(static_cast<moveit_msgs::msg::Constraints>(*msg_m)));
-    //        return true;
-    //      }
-    //      else
-    //        return false;
-    //    }
-    //    else
-    return false;
+    if (constraints_storage_)
+    {
+      moveit_warehouse::ConstraintsWithMetadata msg_m;
+      if (constraints_storage_->getConstraints(msg_m, constraint, robot_model_->getName(), opt_.group_name_))
+      {
+        path_constraints_.reset(new moveit_msgs::msg::Constraints(static_cast<moveit_msgs::msg::Constraints>(*msg_m)));
+        return true;
+      }
+      else
+        return false;
+    }
+    else
+      return false;
   }
 
   void clearPathConstraints()
@@ -1235,9 +1231,8 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
     }
 
     std::vector<std::string> c;
-    // TODO(JafarAbdi): Enable once moveit_ros_warehouse is ported
-    //    if (constraints_storage_)
-    //      constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
+    if (constraints_storage_)
+      constraints_storage_->getKnownConstraints(c, robot_model_->getName(), opt_.group_name_);
 
     return c;
   }
@@ -1288,20 +1283,19 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
   void initializeConstraintsStorageThread(const std::string& host, unsigned int port)
   {
     // Set up db
-    // TODO(JafarAbdi): Enable once moveit_ros_warehouse is ported
-    //    try
-    //    {
-    //      warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase();
-    //      conn->setParams(host, port);
-    //      if (conn->connect())
-    //      {
-    //        constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn));
-    //      }
-    //    }
-    //    catch (std::exception& ex)
-    //    {
-    //      RCLCPP_ERROR(LOGGER, "%s", ex.what());
-    //    }
+    try
+    {
+      warehouse_ros::DatabaseConnection::Ptr conn = moveit_warehouse::loadDatabase(node_);
+      conn->setParams(host, port);
+      if (conn->connect())
+      {
+        constraints_storage_.reset(new moveit_warehouse::ConstraintsStorage(conn));
+      }
+    }
+    catch (std::exception& ex)
+    {
+      RCLCPP_ERROR(LOGGER, "%s", ex.what());
+    }
     initializing_constraints_ = false;
   }
 
@@ -1356,8 +1350,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
   rclcpp::Client<moveit_msgs::srv::SetPlannerParams>::SharedPtr set_params_service_;
   rclcpp::Client<moveit_msgs::srv::GetCartesianPath>::SharedPtr cartesian_path_service_;
   // rclcpp::Client<moveit_msgs::srv::GraspPlanning>::SharedPtr plan_grasps_service_;
-  // TODO(JafarAbdi): Enable once moveit_ros_warehouse is ported
-  // std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
+  std::unique_ptr<moveit_warehouse::ConstraintsStorage> constraints_storage_;
   std::unique_ptr<boost::thread> constraints_init_thread_;
   bool initializing_constraints_;
 };

diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml
@@ -22,8 +22,7 @@
 
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning</depend>
-  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_warehouse -->
-  <!--depend>moveit_ros_warehouse</depend-->
+  <depend>moveit_ros_warehouse</depend>
   <depend>moveit_ros_move_group</depend>
   <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_manipulation -->
   <!--depend>moveit_ros_manipulation</depend-->
@@ -44,7 +43,6 @@
   <test_depend>moveit_resources_fanuc_moveit_config</test_depend>
   <test_depend>moveit_resources_panda_moveit_config</test_depend>
   <test_depend>moveit_simple_controller_manager</test_depend>
-  <test_depend>warehouse_ros_mongo</test_depend>
   <test_depend>moveit_planners_ompl</test_depend>
   <test_depend>rviz2</test_depend>
   <test_depend>ros_testing</test_depend>