basic_grasping_perception.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013-2014, Unbounded Robotics Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Unbounded Robotics, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 // Author: Michael Ferguson
31 
32 #include <ros/ros.h>
34 #include <tf/transform_listener.h>
35 
38 #include <grasping_msgs/FindGraspableObjectsAction.h>
39 
40 #include <pcl_ros/point_cloud.h>
41 #include <pcl_ros/transforms.h>
42 #include <pcl/filters/passthrough.h>
44 
45 namespace simple_grasping
46 {
47 
52 {
54 
55 public:
57  {
58  // use_debug: enable/disable output of a cloud containing object points
59  nh_.getParam("use_debug", debug_);
60 
61  // frame_id: frame to transform cloud to (should be XY horizontal)
62  world_frame_ = "base_link";
63  nh_.getParam("frame_id", world_frame_);
64 
65  // Create planner
66  planner_.reset(new ShapeGraspPlanner(nh_));
67 
68  // Create perception
70 
71  // Advertise an action for perception + planning
72  server_.reset(new server_t(nh_, "find_objects",
73  boost::bind(&BasicGraspingPerception::executeCallback, this, _1),
74  false));
75 
76  // Publish debugging views
77  if (debug_)
78  {
79  object_cloud_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("object_cloud", 1);
80  support_cloud_pub_ = nh_.advertise< pcl::PointCloud<pcl::PointXYZRGB> >("support_cloud", 1);
81  }
82 
83  // Range filter for cloud
84  range_filter_.setFilterFieldName("z");
85  range_filter_.setFilterLimits(0, 2.5);
86 
87  // Subscribe to head camera cloud
88  cloud_sub_ = nh_.subscribe< pcl::PointCloud<pcl::PointXYZRGB> >("/head_camera/depth_registered/points",
89  1,
91  this);
92 
93  // Start thread for action
94  server_->start();
95 
96  ROS_INFO("basic_grasping_perception initialized");
97  }
98 
99 private:
100  void cloudCallback(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& cloud)
101  {
102  // be lazy
103  if (!find_objects_)
104  return;
105 
106  ROS_DEBUG("Cloud recieved with %d points.", static_cast<int>(cloud->points.size()));
107 
108  // Filter out noisy long-range points
109  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
110  range_filter_.setInputCloud(cloud);
111  range_filter_.filter(*cloud_filtered);
112  ROS_DEBUG("Filtered for range, now %d points.", static_cast<int>(cloud_filtered->points.size()));
113 
114  // Transform to grounded
115  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZRGB>);
116  if (!pcl_ros::transformPointCloud(world_frame_, *cloud_filtered, *cloud_transformed, listener_))
117  {
118  ROS_ERROR("Error transforming to frame %s", world_frame_.c_str());
119  return;
120  }
121 
122  // Run segmentation
123  objects_.clear();
124  supports_.clear();
125  pcl::PointCloud<pcl::PointXYZRGB> object_cloud;
126  pcl::PointCloud<pcl::PointXYZRGB> support_cloud;
127  if (debug_)
128  {
129  object_cloud.header.frame_id = cloud_transformed->header.frame_id;
130  support_cloud.header.frame_id = cloud_transformed->header.frame_id;
131  }
132  segmentation_->segment(cloud_transformed, objects_, supports_, object_cloud, support_cloud, debug_);
133 
134  if (debug_)
135  {
136  object_cloud_pub_.publish(object_cloud);
137  support_cloud_pub_.publish(support_cloud);
138  }
139 
140  // Ok to continue processing
141  find_objects_ = false;
142  }
143 
144  void executeCallback(const grasping_msgs::FindGraspableObjectsGoalConstPtr& goal)
145  {
146  grasping_msgs::FindGraspableObjectsResult result;
147 
148  // Get objects
149  find_objects_ = true;
151  while (find_objects_ == true)
152  {
153  ros::Duration(1/50.0).sleep();
154  if (ros::Time::now() - t > ros::Duration(3.0))
155  {
156  find_objects_ = false;
157  server_->setAborted(result, "Failed to get camera data in alloted time.");
158  ROS_ERROR("Failed to get camera data in alloted time.");
159  return;
160  }
161  }
162 
163  // Set object results
164  for (size_t i = 0; i < objects_.size(); ++i)
165  {
166  grasping_msgs::GraspableObject g;
167  g.object = objects_[i];
168  if (goal->plan_grasps)
169  {
170  // Plan grasps for object
171  planner_->plan(objects_[i], g.grasps);
172  }
173  result.objects.push_back(g);
174  }
175  // Set support surfaces
176  result.support_surfaces = supports_;
177 
178  server_->setSucceeded(result, "Succeeded.");
179  }
180 
182 
183  bool debug_;
184 
186  std::string world_frame_;
187 
189  std::vector<grasping_msgs::Object> objects_;
190  std::vector<grasping_msgs::Object> supports_;
191 
195 
198 
200 
201  pcl::PassThrough<pcl::PointXYZRGB> range_filter_;
202 };
203 
204 } // namespace simple_grasping
205 
206 int main(int argc, char* argv[])
207 {
208  ros::init(argc, argv, "basic_grasping_perception");
209  ros::NodeHandle n("~");
210  simple_grasping::BasicGraspingPerception perception_n_planning(n);
211  ros::spin();
212  return 0;
213 }
ROS wrapper for shape grasp planner + object support segmentation.
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Class that segments a point cloud into objects and supporting surfaces.
ROSCPP_DECL void spin(Spinner &spinner)
pcl::PassThrough< pcl::PointXYZRGB > range_filter_
void executeCallback(const grasping_msgs::FindGraspableObjectsGoalConstPtr &goal)
std::vector< grasping_msgs::Object > objects_
int main(int argc, char *argv[])
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::shared_ptr< ObjectSupportSegmentation > segmentation_
boost::shared_ptr< ShapeGraspPlanner > planner_
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
actionlib::SimpleActionServer< grasping_msgs::FindGraspableObjectsAction > server_t
bool getParam(const std::string &key, std::string &s) const
std::vector< grasping_msgs::Object > supports_
static Time now()
void cloudCallback(const pcl::PointCloud< pcl::PointXYZRGB >::ConstPtr &cloud)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)
A simple grasp planner that uses the bounding box shape to generate viable grasps.


simple_grasping
Author(s): Michael Ferguson
autogenerated on Thu Jan 14 2021 03:20:55