From 132c62ae33a503c1adadac1331c438d8e5017fd8 Mon Sep 17 00:00:00 2001 From: Andrea Ponza Date: Fri, 11 Oct 2019 12:50:31 +0200 Subject: [PATCH 1/3] pass through ROS code formatter --- include/gpd_ros/grasp_detection_node.h | 24 +++---- include/gpd_ros/grasp_detection_server.h | 7 +- include/gpd_ros/grasp_messages.h | 7 +- include/gpd_ros/grasp_plotter.h | 15 ++-- launch/ur5.launch | 6 +- src/gpd_ros/grasp_detection_node.cpp | 87 +++++++++++++----------- src/gpd_ros/grasp_detection_server.cpp | 14 ++-- src/gpd_ros/grasp_messages.cpp | 6 +- src/gpd_ros/grasp_plotter.cpp | 38 +++++------ 9 files changed, 99 insertions(+), 105 deletions(-) diff --git a/include/gpd_ros/grasp_detection_node.h b/include/gpd_ros/grasp_detection_node.h index 4b740c3..9807dc7 100644 --- a/include/gpd_ros/grasp_detection_node.h +++ b/include/gpd_ros/grasp_detection_node.h @@ -75,8 +75,7 @@ typedef pcl::PointCloud PointCloudPointNormal; * \brief A ROS node that can detect grasp poses in a point cloud. * * This class is a ROS node that handles all the ROS topics. - * -*/ + */ class GraspDetectionNode { public: @@ -84,12 +83,12 @@ class GraspDetectionNode /** * \brief Constructor. * \param node the ROS node - */ + */ GraspDetectionNode(ros::NodeHandle& node); /** * \brief Destructor. - */ + */ ~GraspDetectionNode() { delete cloud_camera_; @@ -100,16 +99,15 @@ class GraspDetectionNode /** * \brief Run the ROS node. Loops while waiting for incoming ROS messages. - */ + */ void run(); /** * \brief Detect grasp poses in a point cloud received from a ROS topic. * \return the list of grasp poses - */ + */ std::vector> detectGraspPoses(); - private: /** @@ -118,25 +116,25 @@ class GraspDetectionNode * \param centroid the centroid of the ball * \param radius the radius of the ball * \return the indices of the points in the point cloud that lie within the ball - */ + */ std::vector getSamplesInBall(const PointCloudRGBA::Ptr& cloud, const pcl::PointXYZRGBA& centroid, float radius); /** * \brief Callback function for the ROS topic that contains the input point cloud. * \param msg the incoming ROS message - */ + */ void cloud_callback(const sensor_msgs::PointCloud2& msg); /** * \brief Callback function for the ROS topic that contains the input point cloud and a list of indices. * \param msg the incoming ROS message - */ + */ void cloud_indexed_callback(const gpd_ros::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_ros::CloudSamples& msg); /** @@ -148,7 +146,7 @@ class GraspDetectionNode /** * \brief Callback function for the ROS topic that contains the input samples. * \param msg the incoming ROS message - */ + */ void samples_callback(const gpd_ros::SamplesMsg& msg); Eigen::Matrix3Xd fillMatrixFromFile(const std::string& filename, int num_normals); @@ -171,7 +169,7 @@ class GraspDetectionNode std::vector workspace_; ///< workspace limits gpd::GraspDetector* grasp_detector_; ///< used to run the GPD algorithm -//gpd::SequentialImportanceSampling* importance_sampling_; ///< sequential importance sampling variation of GPD algorithm +// gpd::SequentialImportanceSampling* importance_sampling_; ///< sequential importance sampling variation of GPD algorithm GraspPlotter* rviz_plotter_; ///< used to plot detected grasps in rviz /** constants for input point cloud types */ diff --git a/include/gpd_ros/grasp_detection_server.h b/include/gpd_ros/grasp_detection_server.h index 411d3f4..402348b 100644 --- a/include/gpd_ros/grasp_detection_server.h +++ b/include/gpd_ros/grasp_detection_server.h @@ -29,11 +29,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef GRASP_DETECTION_SERVER_H_ #define GRASP_DETECTION_SERVER_H_ - // ROS #include #include @@ -66,12 +64,12 @@ class GraspDetectionServer /** * \brief Constructor. * \param node the ROS node - */ + */ GraspDetectionServer(ros::NodeHandle& node); /** * \brief Destructor. - */ + */ ~GraspDetectionServer() { delete cloud_camera_; @@ -86,7 +84,6 @@ class GraspDetectionServer */ bool detectGrasps(gpd_ros::detect_grasps::Request& req, gpd_ros::detect_grasps::Response& res); - private: ros::Publisher grasps_pub_; ///< ROS publisher for grasp list messages diff --git a/include/gpd_ros/grasp_messages.h b/include/gpd_ros/grasp_messages.h index 0aebb82..528e5a7 100644 --- a/include/gpd_ros/grasp_messages.h +++ b/include/gpd_ros/grasp_messages.h @@ -41,9 +41,10 @@ namespace GraspMessages { - gpd_ros::GraspConfigList createGraspListMsg(const std::vector>& hands, const std_msgs::Header& header); +gpd_ros::GraspConfigList createGraspListMsg(const std::vector>& hands, + const std_msgs::Header& header); - gpd_ros::GraspConfig convertToGraspMsg(const gpd::candidate::Hand& hand); -}; +gpd_ros::GraspConfig convertToGraspMsg(const gpd::candidate::Hand& hand); +} #endif /* GRASP_MESSAGES_H_ */ diff --git a/include/gpd_ros/grasp_plotter.h b/include/gpd_ros/grasp_plotter.h index fde1c73..164b865 100644 --- a/include/gpd_ros/grasp_plotter.h +++ b/include/gpd_ros/grasp_plotter.h @@ -29,7 +29,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef GRASP_PLOTTER_H_ #define GRASP_PLOTTER_H_ @@ -43,13 +42,11 @@ #include #include - /** GraspPlotter class * * \brief Draw grasps in rviz. * * This class provides functions to draw grasps in rviz. - * */ class GraspPlotter { @@ -58,7 +55,7 @@ class GraspPlotter /** * \brief Constructor. * \param node the ROS node - */ + */ GraspPlotter(ros::NodeHandle& node, const gpd::candidate::HandGeometry& params); /** @@ -73,8 +70,8 @@ class GraspPlotter * \param hands list of grasps * \param frame_id the name of the frame that the grasp is in */ - visualization_msgs::MarkerArray convertToVisualGraspMsg(const std::vector>& hands, - const std::string& frame_id); + visualization_msgs::MarkerArray convertToVisualGraspMsg( + const std::vector>& hands, const std::string& frame_id); /** * \brief Convert a list of grasps to a ROS message that can be published to rviz. @@ -87,7 +84,7 @@ class GraspPlotter * \param frame_id the name of the frame that the grasp is in */ visualization_msgs::Marker createFingerMarker(const Eigen::Vector3d& center, const Eigen::Matrix3d& rot, - const Eigen::Vector3d& lwh, int id, const std::string& frame_id); + const Eigen::Vector3d& lwh, int id, const std::string& frame_id); /** * \brief Convert a list of grasps to a ROS message that can be published to rviz. @@ -96,8 +93,8 @@ class GraspPlotter * \param frame_id the name of the frame that the grasp is in */ visualization_msgs::Marker createHandBaseMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, - const Eigen::Matrix3d& frame, double length, double height, int id, const std::string& frame_id); - + const Eigen::Matrix3d& frame, double length, double height, int id, + const std::string& frame_id); private: diff --git a/launch/ur5.launch b/launch/ur5.launch index 539092c..0d239ca 100644 --- a/launch/ur5.launch +++ b/launch/ur5.launch @@ -2,9 +2,7 @@ - + @@ -18,7 +16,5 @@ - - diff --git a/src/gpd_ros/grasp_detection_node.cpp b/src/gpd_ros/grasp_detection_node.cpp index 721d60a..520fc98 100644 --- a/src/gpd_ros/grasp_detection_node.cpp +++ b/src/gpd_ros/grasp_detection_node.cpp @@ -1,26 +1,29 @@ #include - /** constants for input point cloud types */ const int GraspDetectionNode::POINT_CLOUD_2 = 0; ///< sensor_msgs/PointCloud2 const int GraspDetectionNode::CLOUD_INDEXED = 1; ///< cloud with indices const int GraspDetectionNode::CLOUD_SAMPLES = 2; ///< cloud with (x,y,z) samples - -GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false), has_normals_(false), - size_left_cloud_(0), has_samples_(true), frame_(""), use_importance_sampling_(false) +GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) + : has_cloud_(false) + , has_normals_(false) + , size_left_cloud_(0) + , has_samples_(true) + , frame_("") + , use_importance_sampling_(false) { printf("Init ....\n"); cloud_camera_ = NULL; - // set camera viewpoint to default origin +// // set camera viewpoint to default origin // std::vector camera_position; // node.getParam("camera_position", camera_position); // view_point_ << camera_position[0], camera_position[1], camera_position[2]; - - // choose sampling method for grasp detection +// +// // choose sampling method for grasp detection // node.param("use_importance_sampling", use_importance_sampling_, false); - +// // if (use_importance_sampling_) // { // importance_sampling_ = new SequentialImportanceSampling(node); @@ -40,26 +43,35 @@ GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false std::string rviz_topic; node.param("rviz_topic", rviz_topic, std::string("plot_grasps")); - if (!rviz_topic.empty()) { + if (!rviz_topic.empty()) + { grasps_rviz_pub_ = node.advertise(rviz_topic, 1); use_rviz_ = true; - } else { + } + else + { use_rviz_ = false; } // subscribe to input point cloud ROS topic - if (cloud_type == POINT_CLOUD_2) { + if (cloud_type == POINT_CLOUD_2) + { cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_callback, this); - } else if (cloud_type == CLOUD_INDEXED) { + } + else if (cloud_type == CLOUD_INDEXED) + { cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_indexed_callback, this); - } else if (cloud_type == CLOUD_SAMPLES) { + } + else if (cloud_type == CLOUD_SAMPLES) + { cloud_sub_ = node.subscribe(cloud_topic, 1, &GraspDetectionNode::cloud_samples_callback, this); - // grasp_detector_->setUseIncomingSamples(true); +// grasp_detector_->setUseIncomingSamples(true); has_samples_ = false; } // subscribe to input samples ROS topic - if (!samples_topic.empty()) { + if (!samples_topic.empty()) + { samples_sub_ = node.subscribe(samples_topic, 1, &GraspDetectionNode::samples_callback, this); has_samples_ = false; } @@ -72,19 +84,21 @@ GraspDetectionNode::GraspDetectionNode(ros::NodeHandle& node) : has_cloud_(false node.getParam("workspace", workspace_); } - void GraspDetectionNode::run() { ros::Rate rate(100); ROS_INFO("Waiting for point cloud to arrive ..."); - while (ros::ok()) { - if (has_cloud_) { + while (ros::ok()) + { + if (has_cloud_) + { // Detect grasps in point cloud. std::vector> grasps = detectGraspPoses(); // Visualize the detected grasps in rviz. - if (use_rviz_) { + if (use_rviz_) + { rviz_plotter_->drawGrasps(grasps, frame_); } @@ -100,7 +114,6 @@ void GraspDetectionNode::run() } } - std::vector> GraspDetectionNode::detectGraspPoses() { // detect grasp poses @@ -112,7 +125,7 @@ std::vector> GraspDetectionNode::detectGra // cloud_camera_->voxelizeCloud(0.003); // cloud_camera_->calculateNormals(4); // grasps = importance_sampling_->detectGrasps(*cloud_camera_); - printf("Error: importance sampling is not supported yet\n"); + printf("Error: importance sampling is not supported yet\n"); } else { @@ -131,9 +144,8 @@ std::vector> GraspDetectionNode::detectGra return grasps; } - std::vector GraspDetectionNode::getSamplesInBall(const PointCloudRGBA::Ptr& cloud, - const pcl::PointXYZRGBA& centroid, float radius) + const pcl::PointXYZRGBA& centroid, float radius) { std::vector indices; std::vector dists; @@ -143,7 +155,6 @@ std::vector GraspDetectionNode::getSamplesInBall(const PointCloudRGBA::Ptr& return indices; } - void GraspDetectionNode::cloud_callback(const sensor_msgs::PointCloud2& msg) { if (!has_cloud_) @@ -151,11 +162,11 @@ void GraspDetectionNode::cloud_callback(const sensor_msgs::PointCloud2& msg) delete cloud_camera_; cloud_camera_ = NULL; - Eigen::Matrix3Xd view_points(3,1); + Eigen::Matrix3Xd view_points(3, 1); view_points.col(0) = view_point_; if (msg.fields.size() == 6 && msg.fields[3].name == "normal_x" && msg.fields[4].name == "normal_y" - && msg.fields[5].name == "normal_z") + && msg.fields[5].name == "normal_z") { PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); pcl::fromROSMsg(msg, *cloud); @@ -177,7 +188,6 @@ void GraspDetectionNode::cloud_callback(const sensor_msgs::PointCloud2& msg) } } - void GraspDetectionNode::cloud_indexed_callback(const gpd_ros::CloudIndexed& msg) { if (!has_cloud_) @@ -186,7 +196,7 @@ void GraspDetectionNode::cloud_indexed_callback(const gpd_ros::CloudIndexed& msg // Set the indices at which to sample grasp candidates. std::vector indices(msg.indices.size()); - for (int i=0; i < indices.size(); i++) + for (int i = 0; i < indices.size(); i++) { indices[i] = msg.indices[i].data; } @@ -195,12 +205,12 @@ void GraspDetectionNode::cloud_indexed_callback(const gpd_ros::CloudIndexed& msg has_cloud_ = true; frame_ = msg.cloud_sources.cloud.header.frame_id; - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " - << msg.indices.size() << " samples"); + ROS_INFO_STREAM( + "Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " << msg.indices.size() + << " samples"); } } - void GraspDetectionNode::cloud_samples_callback(const gpd_ros::CloudSamples& msg) { if (!has_cloud_) @@ -209,7 +219,7 @@ void GraspDetectionNode::cloud_samples_callback(const gpd_ros::CloudSamples& msg // Set the samples at which to sample grasp candidates. Eigen::Matrix3Xd samples(3, msg.samples.size()); - for (int i=0; i < msg.samples.size(); i++) + for (int i = 0; i < msg.samples.size(); i++) { samples.col(i) << msg.samples[i].x, msg.samples[i].y, msg.samples[i].z; } @@ -219,19 +229,19 @@ void GraspDetectionNode::cloud_samples_callback(const gpd_ros::CloudSamples& msg has_samples_ = true; frame_ = msg.cloud_sources.cloud.header.frame_id; - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " - << cloud_camera_->getSamples().cols() << " samples"); + ROS_INFO_STREAM( + "Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " << + cloud_camera_->getSamples().cols() << " samples"); } } - void GraspDetectionNode::samples_callback(const gpd_ros::SamplesMsg& msg) { if (!has_samples_) { Eigen::Matrix3Xd samples(3, msg.samples.size()); - for (int i=0; i < msg.samples.size(); i++) + for (int i = 0; i < msg.samples.size(); i++) { samples.col(i) << msg.samples[i].x, msg.samples[i].y, msg.samples[i].z; } @@ -243,7 +253,6 @@ void GraspDetectionNode::samples_callback(const gpd_ros::SamplesMsg& msg) } } - void GraspDetectionNode::initCloudCamera(const gpd_ros::CloudSources& msg) { // clean up @@ -258,8 +267,8 @@ void GraspDetectionNode::initCloudCamera(const gpd_ros::CloudSources& msg) } // Set point cloud. - if (msg.cloud.fields.size() == 6 && msg.cloud.fields[3].name == "normal_x" - && msg.cloud.fields[4].name == "normal_y" && msg.cloud.fields[5].name == "normal_z") + if (msg.cloud.fields.size() == 6 && msg.cloud.fields[3].name == "normal_x" && msg.cloud.fields[4].name == "normal_y" + && msg.cloud.fields[5].name == "normal_z") { PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); pcl::fromROSMsg(msg.cloud, *cloud); diff --git a/src/gpd_ros/grasp_detection_server.cpp b/src/gpd_ros/grasp_detection_server.cpp index 171b6d9..639726e 100644 --- a/src/gpd_ros/grasp_detection_server.cpp +++ b/src/gpd_ros/grasp_detection_server.cpp @@ -1,6 +1,5 @@ #include - GraspDetectionServer::GraspDetectionServer(ros::NodeHandle& node) { cloud_camera_ = NULL; @@ -33,7 +32,6 @@ GraspDetectionServer::GraspDetectionServer(ros::NodeHandle& node) node.getParam("workspace", workspace_); } - bool GraspDetectionServer::detectGrasps(gpd_ros::detect_grasps::Request& req, gpd_ros::detect_grasps::Response& res) { ROS_INFO("Received service request ..."); @@ -46,13 +44,12 @@ bool GraspDetectionServer::detectGrasps(gpd_ros::detect_grasps::Request& req, gp Eigen::Matrix3Xd view_points(3, cloud_sources.view_points.size()); for (int i = 0; i < cloud_sources.view_points.size(); i++) { - view_points.col(i) << cloud_sources.view_points[i].x, cloud_sources.view_points[i].y, - cloud_sources.view_points[i].z; + view_points.col(i) << cloud_sources.view_points[i].x, cloud_sources.view_points[i].y, cloud_sources.view_points[i].z; } // Set point cloud. if (cloud_sources.cloud.fields.size() == 6 && cloud_sources.cloud.fields[3].name == "normal_x" - && cloud_sources.cloud.fields[4].name == "normal_y" && cloud_sources.cloud.fields[5].name == "normal_z") + && cloud_sources.cloud.fields[4].name == "normal_y" && cloud_sources.cloud.fields[5].name == "normal_z") { PointCloudPointNormal::Ptr cloud(new PointCloudPointNormal); pcl::fromROSMsg(cloud_sources.cloud, *cloud); @@ -84,7 +81,7 @@ bool GraspDetectionServer::detectGrasps(gpd_ros::detect_grasps::Request& req, gp // Set the indices at which to sample grasp candidates. std::vector indices(req.cloud_indexed.indices.size()); - for (int i=0; i < indices.size(); i++) + for (int i = 0; i < indices.size(); i++) { indices[i] = req.cloud_indexed.indices[i].data; } @@ -92,8 +89,9 @@ bool GraspDetectionServer::detectGrasps(gpd_ros::detect_grasps::Request& req, gp frame_ = req.cloud_indexed.cloud_sources.cloud.header.frame_id; - ROS_INFO_STREAM("Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " - << req.cloud_indexed.indices.size() << " samples"); + ROS_INFO_STREAM( + "Received cloud with " << cloud_camera_->getCloudProcessed()->size() << " points, and " << + req.cloud_indexed.indices.size() << " samples"); // 2. Preprocess the point cloud. grasp_detector_->preprocessPointCloud(*cloud_camera_); diff --git a/src/gpd_ros/grasp_messages.cpp b/src/gpd_ros/grasp_messages.cpp index 6a4594c..70c68e3 100644 --- a/src/gpd_ros/grasp_messages.cpp +++ b/src/gpd_ros/grasp_messages.cpp @@ -1,10 +1,12 @@ #include -gpd_ros::GraspConfigList GraspMessages::createGraspListMsg(const std::vector>& hands, const std_msgs::Header& header) +gpd_ros::GraspConfigList GraspMessages::createGraspListMsg( + const std::vector>& hands, const std_msgs::Header& header) { gpd_ros::GraspConfigList msg; - for (int i = 0; i < hands.size(); i++) { + for (int i = 0; i < hands.size(); i++) + { msg.grasps.push_back(convertToGraspMsg(*hands[i])); } diff --git a/src/gpd_ros/grasp_plotter.cpp b/src/gpd_ros/grasp_plotter.cpp index 8698fed..3639272 100644 --- a/src/gpd_ros/grasp_plotter.cpp +++ b/src/gpd_ros/grasp_plotter.cpp @@ -1,6 +1,5 @@ #include - GraspPlotter::GraspPlotter(ros::NodeHandle& node, const gpd::candidate::HandGeometry& params) { std::string rviz_topic; @@ -13,7 +12,6 @@ GraspPlotter::GraspPlotter(ros::NodeHandle& node, const gpd::candidate::HandGeom finger_width_ = params.finger_width_; } - void GraspPlotter::drawGrasps(const std::vector>& hands, const std::string& frame) { visualization_msgs::MarkerArray markers; @@ -21,16 +19,15 @@ void GraspPlotter::drawGrasps(const std::vector>& hands, - const std::string& frame_id) +visualization_msgs::MarkerArray GraspPlotter::convertToVisualGraspMsg( + const std::vector>& hands, const std::string& frame_id) { - double hw = 0.5*outer_diameter_ - 0.5*finger_width_; + double hw = 0.5 * outer_diameter_ - 0.5 * finger_width_; visualization_msgs::MarkerArray marker_array; visualization_msgs::Marker left_finger, right_finger, base, approach; Eigen::Vector3d left_bottom, right_bottom, left_top, right_top, left_center, right_center, approach_center, - base_center; + base_center; for (int i = 0; i < hands.size(); i++) { @@ -38,19 +35,19 @@ visualization_msgs::MarkerArray GraspPlotter::convertToVisualGraspMsg(const std: right_bottom = hands[i]->getPosition() + hw * hands[i]->getBinormal(); left_top = left_bottom + hand_depth_ * hands[i]->getApproach(); right_top = right_bottom + hand_depth_ * hands[i]->getApproach(); - left_center = left_bottom + 0.5*(left_top - left_bottom); - right_center = right_bottom + 0.5*(right_top - right_bottom); - base_center = left_bottom + 0.5*(right_bottom - left_bottom) - 0.01*hands[i]->getApproach(); - approach_center = base_center - 0.04*hands[i]->getApproach(); + left_center = left_bottom + 0.5 * (left_top - left_bottom); + right_center = right_bottom + 0.5 * (right_top - right_bottom); + base_center = left_bottom + 0.5 * (right_bottom - left_bottom) - 0.01 * hands[i]->getApproach(); + approach_center = base_center - 0.04 * hands[i]->getApproach(); Eigen::Vector3d finger_lwh, approach_lwh; finger_lwh << hand_depth_, finger_width_, hand_height_; approach_lwh << 0.08, finger_width_, hand_height_; base = createHandBaseMarker(left_bottom, right_bottom, hands[i]->getFrame(), 0.02, hand_height_, i, frame_id); - left_finger = createFingerMarker(left_center, hands[i]->getFrame(), finger_lwh, i*3, frame_id); - right_finger = createFingerMarker(right_center, hands[i]->getFrame(), finger_lwh, i*3+1, frame_id); - approach = createFingerMarker(approach_center, hands[i]->getFrame(), approach_lwh, i*3+2, frame_id); + left_finger = createFingerMarker(left_center, hands[i]->getFrame(), finger_lwh, i * 3, frame_id); + right_finger = createFingerMarker(right_center, hands[i]->getFrame(), finger_lwh, i * 3 + 1, frame_id); + approach = createFingerMarker(approach_center, hands[i]->getFrame(), approach_lwh, i * 3 + 2, frame_id); marker_array.markers.push_back(left_finger); marker_array.markers.push_back(right_finger); @@ -61,9 +58,9 @@ visualization_msgs::MarkerArray GraspPlotter::convertToVisualGraspMsg(const std: return marker_array; } - -visualization_msgs::Marker GraspPlotter::createFingerMarker(const Eigen::Vector3d& center, - const Eigen::Matrix3d& frame, const Eigen::Vector3d& lwh, int id, const std::string& frame_id) +visualization_msgs::Marker GraspPlotter::createFingerMarker(const Eigen::Vector3d& center, const Eigen::Matrix3d& frame, + const Eigen::Vector3d& lwh, int id, + const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; @@ -97,10 +94,9 @@ visualization_msgs::Marker GraspPlotter::createFingerMarker(const Eigen::Vector3 return marker; } - -visualization_msgs::Marker GraspPlotter::createHandBaseMarker(const Eigen::Vector3d& start, - const Eigen::Vector3d& end, const Eigen::Matrix3d& frame, double length, double height, int id, - const std::string& frame_id) +visualization_msgs::Marker GraspPlotter::createHandBaseMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, + const Eigen::Matrix3d& frame, double length, + double height, int id, const std::string& frame_id) { Eigen::Vector3d center = start + 0.5 * (end - start); From dff79597a7381b2d4f5ba663c409e0e24ed24518 Mon Sep 17 00:00:00 2001 From: Andrea Ponza Date: Fri, 11 Oct 2019 15:56:37 +0200 Subject: [PATCH 2/3] fix catkin lint issues --- CMakeLists.txt | 86 +++++++++++++++++++++++++++++++++++++++++--------- package.xml | 85 +++++++++++-------------------------------------- 2 files changed, 89 insertions(+), 82 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bc883e..d54477e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,16 +1,24 @@ cmake_minimum_required(VERSION 2.8.3) project(gpd_ros) +add_compile_options(-std=c++14) + ## 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 message_generation roscpp rospy sensor_msgs std_msgs) +## 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 + pcl_conversions + roscpp + sensor_msgs + std_msgs + visualization_msgs +) # PCL find_package(PCL REQUIRED) -include_directories(${PCL_INCLUDE_DIRS}) -link_directories(${PCL_LIBRARY_DIRS}) -add_definitions(${PCL_DEFINITIONS}) ## System dependencies are found with CMake's conventions find_library(GPD_LIB NAMES gpd PATHS /usr/local/lib PATH_SUFFIXES lib NO_DEFAULT_PATH) @@ -22,11 +30,15 @@ endif() include_directories(${GPD_LIB_INCLUDE_DIR}) message(STATUS "gpd_include_dir: ${GPD_LIB_INCLUDE_DIR}") -set(CMAKE_CXX_FLAGS "-O3 -fopenmp -fPIC -Wno-deprecated -Wenum-compare -Wno-ignored-attributes -std=c++14") - ## Generate messages in the 'msg' folder -add_message_files(FILES CloudIndexed.msg CloudSamples.msg CloudSources.msg GraspConfig.msg GraspConfigList.msg - SamplesMsg.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 detect_grasps.srv) @@ -44,11 +56,22 @@ generate_messages(DEPENDENCIES geometry_msgs sensor_msgs std_msgs) ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -INCLUDE_DIRS include -CATKIN_DEPENDS cmake_modules eigen_conversions geometry_msgs message_runtime roscpp sensor_msgs std_msgs -DEPENDS PCL -# CATKIN_DEPENDS cmake_modules eigen_conversions geometry_msgs message_runtime roscpp sensor_msgs std_msgs -# DEPENDS Eigen OpenCV PCL + INCLUDE_DIRS include + LIBRARIES + ${PROJECT_NAME}_grasp_messages + ${PROJECT_NAME}_grasp_plotter + CATKIN_DEPENDS + cmake_modules + eigen_conversions + geometry_msgs + message_generation + message_runtime + pcl_conversions + roscpp + sensor_msgs + std_msgs + visualization_msgs + DEPENDS PCL ) ########### @@ -91,3 +114,36 @@ target_link_libraries(${PROJECT_NAME}_grasp_plotter target_link_libraries(${PROJECT_NAME}_grasp_messages ${GPD_LIB} ${catkin_LIBRARIES}) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executables and/or libraries for installation +install(TARGETS + ${PROJECT_NAME}_detect_grasps + ${PROJECT_NAME}_detect_grasps_server + ${PROJECT_NAME}_grasp_messages + ${PROJECT_NAME}_grasp_plotter + + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN "*.git" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch + PATTERN ".git" EXCLUDE +) diff --git a/package.xml b/package.xml index 42f6f0c..5062a83 100644 --- a/package.xml +++ b/package.xml @@ -1,80 +1,31 @@ - + + gpd_ros 0.0.0 - The gpd_ros package + + wrapper around the GPD package for detecting 6-DOF grasp poses for a + 2-finger robot hand (e.g. a parallel jaw gripper) in 3D point clouds. + - - - ur5 + BSD - - - - TODO - - - - - - + catkin + cmake_modules + eigen_conversions + geometry_msgs + pcl_conversions + roscpp + sensor_msgs + std_msgs + visualization_msgs - - - - + message_generation + message_generation - - - - - - - - - - - - - - - - - - - - - catkin - - cmake_modules - eigen_conversions - geometry_msgs - message_generation - roscpp - rospy - sensor_msgs - std_msgs - - roscpp - rospy - - cmake_modules - eigen_conversions - geometry_msgs message_runtime - roscpp - rospy - sensor_msgs - std_msgs - - - - - - - From f1666ba583995d53175032d6500d9258a1088f60 Mon Sep 17 00:00:00 2001 From: Andrea Ponza Date: Mon, 14 Oct 2019 13:04:04 +0200 Subject: [PATCH 3/3] fix message generation --- CMakeLists.txt | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d54477e..f2b2307 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,9 +84,11 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS}) ## Declare a C++ library add_library(${PROJECT_NAME}_grasp_messages src/${PROJECT_NAME}/grasp_messages.cpp) -add_dependencies(${PROJECT_NAME}_grasp_messages ${catkin_EXPORTED_TARGETS}) add_library(${PROJECT_NAME}_grasp_plotter src/${PROJECT_NAME}/grasp_plotter.cpp) +## Add cmake target dependencies of the library (message generation needs to be built before libraries) +add_dependencies(${PROJECT_NAME}_grasp_messages ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + ## Declare a C++ executable add_executable(${PROJECT_NAME}_detect_grasps src/gpd_ros/grasp_detection_node.cpp) add_executable(${PROJECT_NAME}_detect_grasps_server src/gpd_ros/grasp_detection_server.cpp)