Exposing service inside gpd_node to set the workspace, workspace_gras…

…p, and camera_position parameters
jeoseo · May 9, 2018 · 6c129dd · 6c129dd
1 parent 52f947b
commit 6c129dd
Show file tree

Hide file tree

Showing 4 changed files with 151 additions and 75 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -4,7 +4,7 @@ project(gpd)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS cmake_modules eigen_conversions geometry_msgs message_generation roscpp 
+find_package(catkin REQUIRED COMPONENTS cmake_modules eigen_conversions geometry_msgs message_generation roscpp
   sensor_msgs std_msgs)
 
 # PCL
@@ -67,15 +67,14 @@ set(CMAKE_CXX_FLAGS "-DNDEBUG -O3 -fopenmp -flto -Wno-deprecated -Wenum-compare"
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-add_message_files(FILES CloudIndexed.msg CloudSamples.msg CloudSources.msg GraspConfig.msg GraspConfigList.msg 
+add_message_files(FILES CloudIndexed.msg CloudSamples.msg CloudSources.msg GraspConfig.msg GraspConfigList.msg
   SamplesMsg.msg)
 
 ## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+add_service_files(DIRECTORY srv
+ FILES
+   SetParameters.srv
+)
 
 ## Generate actions in the 'action' folder
 # add_action_files(
@@ -159,98 +158,98 @@ add_executable(${PROJECT_NAME}_test_occlusion src/tests/test_occlusion.cpp)
 # add_dependencies(grasp_candidates_classifier_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME}_caffe_classifier 
-                      ${Caffe_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_caffe_classifier
+                      ${Caffe_LIBRARIES}
                       ${OpenCV_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_clustering 
+target_link_libraries(${PROJECT_NAME}_clustering
                       ${GENERATOR_LIB})
 
-target_link_libraries(${PROJECT_NAME}_create_grasp_images 
-                      ${PROJECT_NAME}_learning 
-                      ${GENERATOR_LIB} 
-                      ${catkin_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_create_grasp_images
+                      ${PROJECT_NAME}_learning
+                      ${GENERATOR_LIB}
+                      ${catkin_LIBRARIES}
                       ${PCL_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_create_training_data  
+target_link_libraries(${PROJECT_NAME}_create_training_data
                       ${PROJECT_NAME}_data_generator)
 
-target_link_libraries(${PROJECT_NAME}_data_generator   
-                      ${PROJECT_NAME}_learning 
-                      ${GENERATOR_LIB} 
-                      ${Caffe_LIBRARIES} 
-                      ${catkin_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_data_generator
+                      ${PROJECT_NAME}_learning
+                      ${GENERATOR_LIB}
+                      ${Caffe_LIBRARIES}
+                      ${catkin_LIBRARIES}
                       ${PCL_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_generate_candidates 
-                      ${GENERATOR_LIB} 
-                      ${catkin_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_generate_candidates
+                      ${GENERATOR_LIB}
+                      ${catkin_LIBRARIES}
                       ${PCL_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_classify_candidates                       
-                      ${PROJECT_NAME}_grasp_detector 
-                      ${PROJECT_NAME}_sequential_importance_sampling 
-                      ${GENERATOR_LIB} 
-                      ${catkin_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_classify_candidates
+                      ${PROJECT_NAME}_grasp_detector
+                      ${PROJECT_NAME}_sequential_importance_sampling
+                      ${GENERATOR_LIB}
+                      ${catkin_LIBRARIES}
                       ${PCL_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_grasp_detector 
-                      ${PROJECT_NAME}_caffe_classifier 
-                      ${PROJECT_NAME}_clustering 
-                      ${PROJECT_NAME}_learning 
-                      ${GENERATOR_LIB} 
-                      ${catkin_LIBRARIES} 
+target_link_libraries(${PROJECT_NAME}_grasp_detector
+                      ${PROJECT_NAME}_caffe_classifier
+                      ${PROJECT_NAME}_clustering
+                      ${PROJECT_NAME}_learning
+                      ${GENERATOR_LIB}
+                      ${catkin_LIBRARIES}
                       ${Caffe_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_grasp_image 
+target_link_libraries(${PROJECT_NAME}_grasp_image
                       ${OpenCV_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_grasp_image_15_channels 
+target_link_libraries(${PROJECT_NAME}_grasp_image_15_channels
                       ${PROJECT_NAME}_grasp_image)
 
-target_link_libraries(${PROJECT_NAME}_learning 
-                      ${PROJECT_NAME}_grasp_image_15_channels 
+target_link_libraries(${PROJECT_NAME}_learning
+                      ${PROJECT_NAME}_grasp_image_15_channels
                       ${GENERATOR_LIB})
 
-target_link_libraries(${PROJECT_NAME}_detect_grasps 
+target_link_libraries(${PROJECT_NAME}_detect_grasps
                       ${PROJECT_NAME}_grasp_detector
-                      ${PROJECT_NAME}_sequential_importance_sampling  
-                      ${GENERATOR_LIB}  
+                      ${PROJECT_NAME}_sequential_importance_sampling
+                      ${GENERATOR_LIB}
                       ${PCL_LIBRARIES})
 
-target_link_libraries(${PROJECT_NAME}_sequential_importance_sampling 
+target_link_libraries(${PROJECT_NAME}_sequential_importance_sampling
                       ${PROJECT_NAME}_grasp_detector)
 
 target_link_libraries(${PROJECT_NAME}_test_occlusion
                       ${PROJECT_NAME}_learning
-                      ${GENERATOR_LIB} 
+                      ${GENERATOR_LIB}
                       ${catkin_LIBRARIES}
-                      ${PCL_LIBRARIES} 
+                      ${PCL_LIBRARIES}
                       ${Caffe_LIBRARIES})
 
 target_link_libraries(${PROJECT_NAME}_learning
-                      ${GENERATOR_LIB} 
-                      ${catkin_LIBRARIES} 
+                      ${GENERATOR_LIB}
+                      ${catkin_LIBRARIES}
                       ${Caffe_LIBRARIES})
 
 
 # Rename targets to simplify their names.
-set_target_properties(${PROJECT_NAME}_detect_grasps 
-                      PROPERTIES OUTPUT_NAME detect_grasps 
+set_target_properties(${PROJECT_NAME}_detect_grasps
+                      PROPERTIES OUTPUT_NAME detect_grasps
                       PREFIX "")
 
-set_target_properties(${PROJECT_NAME}_classify_candidates 
-                      PROPERTIES OUTPUT_NAME classify_candidates 
+set_target_properties(${PROJECT_NAME}_classify_candidates
+                      PROPERTIES OUTPUT_NAME classify_candidates
                       PREFIX "")
 
 set_target_properties(${PROJECT_NAME}_create_grasp_images
-                      PROPERTIES OUTPUT_NAME create_grasp_images  
-                      PREFIX "")                      
+                      PROPERTIES OUTPUT_NAME create_grasp_images
+                      PREFIX "")
+
+set_target_properties(${PROJECT_NAME}_create_training_data
+                      PROPERTIES OUTPUT_NAME create_training_data
+                      PREFIX "")
 
-set_target_properties(${PROJECT_NAME}_create_training_data 
-                      PROPERTIES OUTPUT_NAME create_training_data 
-                      PREFIX "")                      
-
 set_target_properties(${PROJECT_NAME}_test_occlusion
-                      PROPERTIES OUTPUT_NAME test_occlusion 
-                      PREFIX "")                     
+                      PROPERTIES OUTPUT_NAME test_occlusion
+                      PREFIX "")
diff --git a/include/nodes/grasp_detection_node.h b/include/nodes/grasp_detection_node.h
@@ -64,6 +64,7 @@
 #include "../gpd/grasp_detector.h"
 #include "../gpd/sequential_importance_sampling.h"
 
+#include <gpd/SetParameters.h>
 
 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloudRGBA;
 typedef pcl::PointCloud<pcl::PointNormal> PointCloudPointNormal;
@@ -79,7 +80,7 @@ typedef pcl::PointCloud<pcl::PointNormal> PointCloudPointNormal;
 class GraspDetectionNode
 {
 public:
-  
+
   /**
    * \brief Constructor.
    * \param node the ROS node
@@ -135,13 +136,19 @@ class GraspDetectionNode
    * \param msg the incoming ROS message
   */
   void cloud_indexed_callback(const gpd::CloudIndexed& msg);
-  
+
   /**
    * \brief Callback function for the ROS topic that contains the input point cloud and a list of (x,y,z) samples.
    * \param msg the incoming ROS message
   */
   void cloud_samples_callback(const gpd::CloudSamples& msg);
 
+  /**
+   * \brief Callback function for the ROS service that reloads the rosparams.
+   * \param req, resp the service request and response
+  */
+  bool set_params_callback(gpd::SetParameters::Request &req, gpd::SetParameters::Response &resp);
+
   /**
    * \brief Initialize the <cloud_camera> object given a <cloud_sources> message.
    * \param msg the <cloud_sources> message
@@ -182,10 +189,13 @@ class GraspDetectionNode
   int size_left_cloud_; ///< (input) size of the left point cloud (when using two point clouds as input)
   bool has_cloud_, has_normals_, has_samples_; ///< status variables for received (input) messages
   std::string frame_; ///< point cloud frame
+
+  ros::NodeHandle nh_; ///< ROS node handle
   ros::Subscriber cloud_sub_; ///< ROS subscriber for point cloud messages
   ros::Subscriber samples_sub_; ///< ROS subscriber for samples messages
   ros::Publisher grasps_pub_; ///< ROS publisher for grasp list messages
   ros::Publisher grasps_rviz_pub_; ///< ROS publisher for grasps in rviz (visualization)
+  ros::ServiceServer srv_set_params_; ///< ROS service server for setting params
 
   bool use_importance_sampling_; ///< if importance sampling is used
   bool filter_grasps_; ///< if grasps are filtered on workspace and gripper aperture

diff --git a/src/nodes/grasp_detection_node.cpp b/src/nodes/grasp_detection_node.cpp
@@ -12,33 +12,35 @@ GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false
 {
   cloud_camera_ = NULL;
 
+  nh_ = node; // Assign the NodeHandle to the private variable
+
   // set camera viewpoint to default origin
   std::vector<double> camera_position;
-  node.getParam("camera_position", camera_position);
+  nh_.getParam("camera_position", camera_position);
   view_point_ << camera_position[0], camera_position[1], camera_position[2];
 
   // choose sampling method for grasp detection
-  node.param("use_importance_sampling", use_importance_sampling_, false);
+  nh_.param("use_importance_sampling", use_importance_sampling_, false);
 
   if (use_importance_sampling_)
   {
-    importance_sampling_ = new SequentialImportanceSampling(node);
+    importance_sampling_ = new SequentialImportanceSampling(nh_);
   }
-  grasp_detector_ = new GraspDetector(node);
+  grasp_detector_ = new GraspDetector(nh_);
 
   // Read input cloud and sample ROS topics parameters.
   int cloud_type;
-  node.param("cloud_type", cloud_type, POINT_CLOUD_2);
+  nh_.param("cloud_type", cloud_type, POINT_CLOUD_2);
   std::string cloud_topic;
-  node.param("cloud_topic", cloud_topic, std::string("/camera/depth_registered/points"));
+  nh_.param("cloud_topic", cloud_topic, std::string("/camera/depth_registered/points"));
   std::string samples_topic;
-  node.param("samples_topic", samples_topic, std::string(""));
+  nh_.param("samples_topic", samples_topic, std::string(""));
   std::string rviz_topic;
-  node.param("rviz_topic", rviz_topic, std::string(""));
+  nh_.param("rviz_topic", rviz_topic, std::string(""));
 
   if (!rviz_topic.empty())
   {
-    grasps_rviz_pub_ = node.advertise<visualization_msgs::MarkerArray>(rviz_topic, 1);
+    grasps_rviz_pub_ = nh_.advertise<visualization_msgs::MarkerArray>(rviz_topic, 1);
     use_rviz_ = true;
   }
   else
@@ -48,27 +50,30 @@ GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false
 
   // subscribe to input point cloud ROS topic
   if (cloud_type == POINT_CLOUD_2)
-    cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_callback, this);
+    cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_callback, this);
   else if (cloud_type == CLOUD_INDEXED)
-    cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_indexed_callback, this);
+    cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_indexed_callback, this);
   else if (cloud_type == CLOUD_SAMPLES)
   {
-    cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_samples_callback, this);
+    cloud_sub_ = nh_.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_samples_callback, this);
     //    grasp_detector_->setUseIncomingSamples(true);
     has_samples_ = false;
   }
 
   // subscribe to input samples ROS topic
   if (!samples_topic.empty())
   {
-    samples_sub_ = node.subscribe(samples_topic, 1, &GraspDetectionNode::samples_callback, this);
+    samples_sub_ = nh_.subscribe(samples_topic, 1, &GraspDetectionNode::samples_callback, this);
     has_samples_ = false;
   }
 
   // uses ROS topics to publish grasp candidates, antipodal grasps, and grasps after clustering
-  grasps_pub_ = node.advertise<gpd::GraspConfigList>("clustered_grasps", 10);
+  grasps_pub_ = nh_.advertise<gpd::GraspConfigList>("clustered_grasps", 10);
+
+  // Advertise the SetParameters service
+  srv_set_params_ = nh_.advertiseService("/gpd/set_params", &GraspDetectionNode::set_params_callback, this);
 
-  node.getParam("workspace", workspace_);
+  nh_.getParam("workspace", workspace_);
 }
 
 
@@ -104,6 +109,57 @@ void GraspDetectionNode::run()
   }
 }
 
+bool GraspDetectionNode::set_params_callback(gpd::SetParameters::Request &req, gpd::SetParameters::Response &resp)
+{
+  // Delete the existing sampler and detectors
+  if (use_importance_sampling_)
+  {
+    delete importance_sampling_;
+  }
+  delete grasp_detector_;
+
+  // Set the workspace from the request
+  if (req.set_workspace)
+  {
+    workspace_.clear();
+    for (int i = 0; i < req.workspace.size(); i++){
+
+      workspace_.push_back(req.workspace[i]);
+    }
+    nh_.setParam("workspace", workspace_);
+  }
+
+  // Set the workspace_grasps from the request
+  if (req.set_workspace_grasps)
+  {
+    std::vector<double> workspace_grasps;
+    for (int i = 0; i < req.workspace_grasps.size(); i++)
+    {
+      workspace_grasps.push_back(req.workspace_grasps[i]);
+    }
+    nh_.setParam("workspace_grasps", workspace_grasps);
+  }
+
+  if (req.set_camera_position)
+  {
+    view_point_ << req.camera_position[0], req.camera_position[1], req.camera_position[2];
+    std::vector<double> camera_position;
+    camera_position.push_back(view_point_.x());
+    camera_position.push_back(view_point_.y());
+    camera_position.push_back(view_point_.z());
+    nh_.setParam("camera_position", camera_position);
+  }
+
+  // Creating new sampler and detector so they load the new rosparams
+  if (use_importance_sampling_)
+  {
+    importance_sampling_ = new SequentialImportanceSampling(nh_);
+  }
+  grasp_detector_ = new GraspDetector(nh_);
+
+  resp.success = true;
+  return true;
+}
 
 std::vector<Grasp> GraspDetectionNode::detectGraspPosesInTopic()
 {

diff --git a/srv/SetParameters.srv b/srv/SetParameters.srv
@@ -0,0 +1,11 @@
+bool set_workspace
+float32[6] workspace
+
+bool set_workspace_grasps
+float32[6] workspace_grasps
+
+bool set_camera_position
+float32[3] camera_position
+---
+
+bool success