hull_contract.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: transform_pointcloud.cpp 30719 2010-07-09 20:28:41Z rusu $
00035  *
00036  */
00037 
00046 // ROS core
00047 #include <ros/ros.h>
00048 
00049 #include "pcl/io/pcd_io.h"
00050 #include "pcl/point_types.h"
00051 #include "pcl/common/common.h"
00052 
00053 #include <tf/transform_listener.h>
00054 #include <visualization_msgs/Marker.h>
00055 
00056 using namespace std;
00057 
00058 class HullContractNode
00059 {
00060 protected:
00061   ros::NodeHandle nh_;
00062 
00063 public:
00064   string output_cloud_topic_, input_cloud_topic_;
00065 
00066   ros::Subscriber sub_;
00067   ros::Publisher pub_;
00068   ros::Publisher vis_pub_;
00069 
00070   sensor_msgs::PointCloud2 output_cloud_;
00071   pcl::PointCloud<pcl::PointXYZ> cloud_in_;
00072   pcl::PointXYZ point_min_;
00073   pcl::PointXYZ point_max_;
00074   pcl::PointXYZ point_center_;
00075   visualization_msgs::Marker marker_;
00076 
00077   double padding_, offset_x_;
00079   HullContractNode  (ros::NodeHandle &n) : nh_(n)
00080   {
00081     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00082     nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
00083     nh_.param("padding", padding_, 0.8);
00084     //how much to offset the contracted hull into x direction
00085     //this hack is needed to get the objects on the front edge of the shelf
00086     nh_.param("offset_x", offset_x_, 0.02);
00087     output_cloud_topic_ = input_cloud_topic_ + "_padded";
00088     sub_ = nh_.subscribe (input_cloud_topic_, 1,  &HullContractNode::cloud_cb, this);
00089     ROS_INFO ("[HullContractNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
00090     pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
00091     ROS_INFO ("[HullContractNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
00092     vis_pub_ = nh_.advertise<visualization_msgs::Marker>( "visualization_marker", 0 );
00093   }
00094 
00096   // cloud_cb (!)
00097   void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
00098   {
00099     pcl::fromROSMsg(*pc, cloud_in_);
00100     //    cloud_in_.width = cloud_in_.width + 2;
00101     getMinMax3D (cloud_in_, point_min_, point_max_);
00102     //Calculate the centroid of the hull
00103     point_center_.x = (point_max_.x + point_min_.x)/2;
00104     point_center_.y = (point_max_.y + point_min_.y)/2;
00105     point_center_.z = (point_max_.z + point_min_.z)/2;
00106     // float min_x, max_y, min_y, z;
00107     // min_x = min_y = FLT_MAX;
00108     // max_y = FLT_MIN;
00109 
00110     for (unsigned long i = 0; i < cloud_in_.points.size(); i++)
00111     {
00112       //hack to preserve the closest edge of the plane
00113       // if (cloud_in_.points[i].y < min_y && cloud_in_.points[i].x < min_x)
00114       //        {
00115       //          min_y = cloud_in_.points[i].y;
00116       //          min_x = cloud_in_.points[i].x;
00117       //          z = cloud_in_.points[i].z;
00118       //        }
00119       // if (cloud_in_.points[i].y > max_y &&  cloud_in_.points[i].x < min_x)
00120       //        {
00121       //          max_y = cloud_in_.points[i].y;
00122       //          min_x = cloud_in_.points[i].x;
00123       //          z = cloud_in_.points[i].z;
00124       //        }
00125       double dist_to_center = sqrt((point_center_.x - cloud_in_.points[i].x) * (point_center_.x - cloud_in_.points[i].x) +
00126                                    (point_center_.y - cloud_in_.points[i].y) * (point_center_.y - cloud_in_.points[i].y));
00127       ROS_DEBUG("[HullContractNode:] Dist to center: %lf", dist_to_center);
00128       double angle;
00129       angle= atan2((cloud_in_.points[i].y - point_center_.y), (cloud_in_.points[i].x - point_center_.x));
00130       double new_dist_to_center = padding_ * dist_to_center;
00131       cloud_in_.points[i].y = point_center_.y + sin(angle) * new_dist_to_center;
00132       cloud_in_.points[i].x = point_center_.x + cos(angle) * new_dist_to_center;
00133       cloud_in_.points[i].x = cloud_in_.points[i].x - offset_x_;
00134     }
00135     //  //hack to preserve the closest edge of the plane
00136     // pcl::PointXYZ minx_miny, minx_maxy;
00137     // minx_miny.x = min_x;
00138     // minx_miny.y = min_y;
00139     // minx_miny.z = z;
00140     // minx_maxy.x = min_x;
00141     // minx_maxy.y = max_y;
00142     // minx_maxy.z = z;
00143     // cloud_in_.points.push_back(minx_maxy);
00144     // cloud_in_.points.push_back(minx_miny);
00145     //end of hack
00146     pcl::toROSMsg (cloud_in_, output_cloud_);
00147     ROS_INFO("[HullContractNode:] Published contracted hull to topic %s", output_cloud_topic_.c_str());
00148     pub_.publish (output_cloud_);
00149 
00150     marker_.header.frame_id = output_cloud_.header.frame_id;
00151     marker_.header.stamp = ros::Time();
00152     marker_.ns = "my_namespace";
00153     marker_.id = 0;
00154     marker_.type = visualization_msgs::Marker::SPHERE;
00155     marker_.action = visualization_msgs::Marker::ADD;
00156     marker_.pose.position.x = point_center_.x;
00157     marker_.pose.position.y = point_center_.y;
00158     marker_.pose.position.z = point_center_.z;
00159     marker_.pose.orientation.x = 0.0;
00160     marker_.pose.orientation.y = 0.0;
00161     marker_.pose.orientation.z = 0.0;
00162     marker_.pose.orientation.w = 1.0;
00163     marker_.scale.x = 0.1;
00164     marker_.scale.y = 0.1;
00165     marker_.scale.z = 0.1;
00166     marker_.color.a = 1.0;
00167     marker_.color.r = 0.0;
00168     marker_.color.g = 1.0;
00169     marker_.color.b = 0.0;
00170     vis_pub_.publish( marker_ );
00171   }
00172 
00173   void publish_center_radius()
00174   {
00175 
00176   }
00177 };
00178 
00179 int main (int argc, char** argv)
00180 {
00181   ros::init (argc, argv, "transform_pointcloud_node");
00182   ros::NodeHandle n("~");
00183   HullContractNode tp(n);
00184   ros::spin ();
00185   return (0);
00186 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_cloud_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Sun Oct 6 2013 11:58:07