Made the plugin names loaded from the param server after every invoca…

…tion.
ros-industrial-attic · Feb 23, 2017 · fbcbd64 · fbcbd64
1 parent ad28658
commit fbcbd64
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 20 deletions.
diff --git a/godel_surface_detection/include/detection/surface_detection.h b/godel_surface_detection/include/detection/surface_detection.h
@@ -140,7 +140,7 @@ class SurfaceDetection
   virtual ~SurfaceDetection();
 
 public:
-  bool init(const std::string& meshing_plugin_name);
+  bool init();
 
   bool load_parameters(const std::string& filename);
   void save_parameters(const std::string& filename);
@@ -191,6 +191,9 @@ class SurfaceDetection
   bool apply_planar_reprojection(const CloudRGB& in, CloudRGB& out);
   bool apply_concave_hull(const CloudRGB& in, pcl::PolygonMesh& mesh);
 
+  std::string getMeshingPluginName() const;
+
+
 public:
   // parameters
   godel_msgs::SurfaceDetectionParameters params_;
@@ -208,8 +211,6 @@ class SurfaceDetection
 
   // counter
   int acquired_clouds_counter_;
-  // Meshing algorithm
-  std::string meshing_plugin_name_;
 };
 } /* end namespace detection */
 } /* namespace godel_surface_detection */

diff --git a/godel_surface_detection/include/services/surface_blending_service.h b/godel_surface_detection/include/services/surface_blending_service.h
@@ -179,6 +179,10 @@ class SurfaceBlendingService
 
   void visualizePaths();
 
+  std::string getBlendToolPlanningPluginName() const;
+
+  std::string getScanToolPlanningPluginName() const;
+
   // Services offered by this class
   ros::ServiceServer surface_detect_server_;
   ros::ServiceServer select_surface_server_;
@@ -249,11 +253,6 @@ class SurfaceBlendingService
 
   // Parameter loading and saving
   std::string param_cache_prefix_;
-
-  // Meshing and Path planning plugin information
-  std::string meshing_plugin_name_;
-  std::string blend_tool_planning_plugin_name_;
-  std::string scan_tool_planning_plugin_name_;
 };
 
 #endif // surface blending services
diff --git a/godel_surface_detection/src/detection/surface_detection.cpp b/godel_surface_detection/src/detection/surface_detection.cpp
@@ -23,6 +23,7 @@
 #include <meshing_plugins_base/meshing_base.h>
 
 const static int DOWNSAMPLE_NUMBER = 3;
+const static std::string MESHING_PLUGIN_PARAM = "meshing_plugin_name";
 
 namespace godel_surface_detection
 {
@@ -68,11 +69,10 @@ namespace godel_surface_detection
     }
 
 
-    bool SurfaceDetection::init(const std::string &meshing_plugin_name)
+    bool SurfaceDetection::init()
     {
       full_cloud_ptr_->header.frame_id = params_.frame_id;
       acquired_clouds_counter_ = 0;
-      meshing_plugin_name_ = meshing_plugin_name;
       return true;
     }
 
@@ -326,7 +326,7 @@ namespace godel_surface_detection
 
       try
       {
-        mesher = poly_loader.createInstance(meshing_plugin_name_);
+        mesher = poly_loader.createInstance(getMeshingPluginName());
 
       }
       catch(pluginlib::PluginlibException& ex)
@@ -365,5 +365,18 @@ namespace godel_surface_detection
 
       return true;
     }
+
+    std::string SurfaceDetection::getMeshingPluginName() const
+    {
+      ros::NodeHandle pnh ("~");
+      std::string name;
+      if (!pnh.getParam(MESHING_PLUGIN_PARAM, name))
+      {
+        ROS_WARN("Unable to load meshing plugin name from ros param '%s'",
+                 MESHING_PLUGIN_PARAM.c_str());
+      }
+
+      return name;
+    }
   } /* end namespace detection */
 } /* end namespace godel_surface_detection */
diff --git a/godel_surface_detection/src/services/blending_service_path_generation.cpp b/godel_surface_detection/src/services/blending_service_path_generation.cpp
@@ -207,7 +207,7 @@ SurfaceBlendingService::generateProcessPath(const int& id,
 
   try
   {
-    path_planner = poly_loader.createInstance(blend_tool_planning_plugin_name_);
+    path_planner = poly_loader.createInstance(getBlendToolPlanningPluginName());
   }
   catch(pluginlib::PluginlibException& ex)
   {
@@ -224,7 +224,7 @@ SurfaceBlendingService::generateProcessPath(const int& id,
 
   try
   {
-    path_planner = poly_loader.createInstance(scan_tool_planning_plugin_name_);
+    path_planner = poly_loader.createInstance(getScanToolPlanningPluginName());
   }
   catch(pluginlib::PluginlibException& ex)
   {

diff --git a/godel_surface_detection/src/services/surface_blending_service.cpp b/godel_surface_detection/src/services/surface_blending_service.cpp
@@ -57,6 +57,11 @@ const static std::string ROBOT_SCAN_PARAMS_FILE = "godel_robot_scan_parameters.m
 const static std::string SURFACE_DETECTION_PARAMS_FILE = "godel_surface_detection_parameters.msg";
 const static std::string PATH_PLANNING_PARAMS_FILE = "godel_path_planning_parameters.msg";
 
+const static std::string BLEND_TOOL_PLUGIN_PARAM = "blend_tool_planning_plugin_name";
+const static std::string SCAN_TOOL_PLUGIN_PARAM = "scan_tool_planning_plugin_name";
+const static std::string MESHING_PLUGIN_PARAM = "meshing_plugin_name";
+
+
 SurfaceBlendingService::SurfaceBlendingService() : publish_region_point_cloud_(false) {}
 
 bool SurfaceBlendingService::init()
@@ -86,24 +91,21 @@ bool SurfaceBlendingService::init()
     ROS_WARN("Unable to load surface detection parameters.");
 
   // load plugins for meshing and tool planning
-  const static std::string MESHING_PLUGIN_PARAM = "meshing_plugin_name";
-  if (!ph.getParam(MESHING_PLUGIN_PARAM, meshing_plugin_name_))
+  if (!ph.hasParam(MESHING_PLUGIN_PARAM))
   {
     ROS_FATAL("SurfaceBlendinService::init(): Expected private parameter '%s' not found. Aborting",
               MESHING_PLUGIN_PARAM.c_str());
     return false;
   }
 
-  const static std::string BLEND_TOOL_PLUGIN_PARAM = "blend_tool_planning_plugin_name";
-  if (!ph.getParam(BLEND_TOOL_PLUGIN_PARAM, blend_tool_planning_plugin_name_))
+  if (!ph.hasParam(BLEND_TOOL_PLUGIN_PARAM))
   {
     ROS_FATAL("SurfaceBlendinService::init(): Expected private parameter '%s' not found. Aborting",
               MESHING_PLUGIN_PARAM.c_str());
     return false;
   }
 
-  const static std::string SCAN_TOOL_PLUGIN_PARAM = "scan_tool_planning_plugin_name";
-  if (!ph.getParam(SCAN_TOOL_PLUGIN_PARAM, scan_tool_planning_plugin_name_))
+  if (!ph.hasParam(SCAN_TOOL_PLUGIN_PARAM))
   {
     ROS_FATAL("SurfaceBlendinService::init(): Expected private parameter '%s' not found. Aborting",
               SCAN_TOOL_PLUGIN_PARAM.c_str());
@@ -116,7 +118,7 @@ bool SurfaceBlendingService::init()
   default_blending_plan_params_ = blending_plan_params_;
   default_scan_params_ = scan_plan_params_;
 
-  if (surface_detection_.init(meshing_plugin_name_) && robot_scan_.init() &&
+  if (surface_detection_.init() && robot_scan_.init() &&
       surface_server_.init() && data_coordinator_.init())
   {
     // adding callbacks
@@ -1003,6 +1005,32 @@ void SurfaceBlendingService::visualizePaths()
   tool_path_markers_pub_.publish(path_visualization);
 }
 
+std::string SurfaceBlendingService::getBlendToolPlanningPluginName() const
+{
+  ros::NodeHandle pnh ("~");
+  std::string name;
+  if (!pnh.getParam(BLEND_TOOL_PLUGIN_PARAM, name))
+  {
+    ROS_WARN("Unable to load blend tool planning plugin from ros param '%s'",
+             BLEND_TOOL_PLUGIN_PARAM.c_str());
+  }
+
+  return name;
+}
+
+std::string SurfaceBlendingService::getScanToolPlanningPluginName() const
+{
+  ros::NodeHandle pnh ("~");
+  std::string name;
+  if (!pnh.getParam(SCAN_TOOL_PLUGIN_PARAM, name))
+  {
+    ROS_WARN("Unable to load scan tool planning plugin from ros param '%s'",
+             SCAN_TOOL_PLUGIN_PARAM.c_str());
+  }
+
+  return name;
+}
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "surface_blending_service");