pointcloud_line_pose.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 
00047 // ROS core
00048 #include <ros/ros.h>
00049 
00050 #include "pcl/io/pcd_io.h"
00051 #include "pcl/point_types.h"
00052 #include "pcl/common/common.h"
00053 #include "pcl/ModelCoefficients.h"
00054 
00055 #include <tf/transform_listener.h>
00056 #include <geometry_msgs/PoseStamped.h>
00057 
00058 using namespace std;
00059 
00060 class PointcloudLinePose
00061 {
00062 protected:
00063   ros::NodeHandle nh_;
00064 
00065 public:
00066   string output_pose_topic_, input_model_topic_;
00067 
00068   ros::Subscriber sub_;
00069   ros::Publisher pub_;
00070 
00071   pcl::ModelCoefficients model_in_;
00072   //handle center pose
00073   geometry_msgs::PoseStamped handle_pose_;
00074 
00075   //sleep one second between publishing of handle poses to
00076   bool sleep_;
00078   PointcloudLinePose  (ros::NodeHandle &n) : nh_(n)
00079   {
00080     // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
00081     nh_.param("input_model_topic", input_model_topic_, std::string("model"));
00082     nh_.param("output_pose_topic", output_pose_topic_, std::string("handle_pose"));
00083     nh_.param("sleep", sleep_, false);
00084     //5 cm between cluster
00085     sub_ = nh_.subscribe (input_model_topic_, 10,  &PointcloudLinePose::model_cb, this);
00086     ROS_INFO ("[PointcloudLinePose:] Listening for incoming data on topic %s.", nh_.resolveName (input_model_topic_).c_str ());
00087     pub_ = nh_.advertise<geometry_msgs::PoseStamped>(output_pose_topic_, 10);
00088     ROS_INFO ("[PointcloudLinePose:] Will be publishing data on topic %s.", nh_.resolveName (output_pose_topic_).c_str ());
00089   }
00090 
00092   // cloud_cb (!)
00093   void model_cb (const pcl::ModelCoefficientsConstPtr& model)
00094   {
00095     handle_pose_.header = model->header;
00096     handle_pose_.pose.position.x = model->values[0];
00097     handle_pose_.pose.position.y = model->values[1];
00098     handle_pose_.pose.position.z = model->values[2];
00099 
00100     //axis-angle rotation
00101     btVector3 axis(model->values[3], model->values[4], model->values[5]);
00102     btVector3 marker_axis(1, 0, 0);
00103     btQuaternion qt(marker_axis.cross(axis.normalize()), marker_axis.angle(axis.normalize()));
00104     geometry_msgs::Quaternion quat_msg;
00105     tf::quaternionTFToMsg(qt, quat_msg);
00106     handle_pose_.pose.orientation = quat_msg;
00107     ROS_INFO("[PointcloudLinePose:] Published cluster to topic %s", output_pose_topic_.c_str());
00108     if (sleep_)
00109       sleep(1.0);
00110     pub_.publish (handle_pose_);
00111   }
00112 
00113 //   void compute_marker(visualization_msgs::Marker &marker,  geometry_msgs::Point32 &point)
00114 //   {
00115 //     marker.header.frame_id = output_cluster_.header.frame_id;
00116 //     marker.header.stamp =  output_cluster_.header.stamp;
00117 //     marker.ns = "object_cluster";
00118 //     marker.id = 0;
00119 //     marker.type = visualization_msgs::Marker::SPHERE;
00120 //     marker.action = visualization_msgs::Marker::ADD;
00121 //     marker.pose.position.x = point.x;
00122 //     marker.pose.position.y = point.y;
00123 //     marker.pose.position.z = point.z;
00124 //     marker.pose.orientation.x = 0.0;
00125 //     marker.pose.orientation.y = 0.0;
00126 //     marker.pose.orientation.z = 0.0;
00127 //     marker.pose.orientation.w = 1.0;
00128 //     marker.scale.x = 0.05;
00129 //     marker.scale.y = 0.05;
00130 //     marker.scale.z = 0.05;
00131 //     marker.color.a = 1.0;
00132 //     marker.color.r = 0.0;
00133 //     marker.color.g = 1.0;
00134 //     marker.color.b = 0.0;
00135 
00136 //   }
00137 };
00138 
00139 int main (int argc, char** argv)
00140 {
00141   ros::init (argc, argv, "pointcloud_line_pose");
00142   ros::NodeHandle n("~");
00143   PointcloudLinePose lp(n);
00144   ros::spin ();
00145   return (0);
00146 }
 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 Thu May 23 2013 17:11:36