From 6c129dd639f8655c6c96b88ff9354473f3a15d3f Mon Sep 17 00:00:00 2001 From: = Date: Wed, 9 May 2018 12:31:03 -0400 Subject: [PATCH] Exposing service inside gpd_node to set the workspace, workspace_grasp, and camera_position parameters --- CMakeLists.txt | 115 +++++++++++++-------------- include/nodes/grasp_detection_node.h | 14 +++- src/nodes/grasp_detection_node.cpp | 86 ++++++++++++++++---- srv/SetParameters.srv | 11 +++ 4 files changed, 151 insertions(+), 75 deletions(-) create mode 100644 srv/SetParameters.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index f246420..77f77dc 100644 --- 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 index eb6f908..5a543bc 100644 --- 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 typedef pcl::PointCloud PointCloudRGBA; typedef pcl::PointCloud PointCloudPointNormal; @@ -79,7 +80,7 @@ typedef pcl::PointCloud 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 object given a message. * \param msg the 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 index bc29c41..d028e95 100644 --- 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 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(rviz_topic, 1); + grasps_rviz_pub_ = nh_.advertise(rviz_topic, 1); use_rviz_ = true; } else @@ -48,12 +50,12 @@ 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; } @@ -61,14 +63,17 @@ GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(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("clustered_grasps", 10); + grasps_pub_ = nh_.advertise("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 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 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 GraspDetectionNode::detectGraspPosesInTopic() { diff --git a/srv/SetParameters.srv b/srv/SetParameters.srv new file mode 100644 index 0000000..eeb527f --- /dev/null +++ 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