point_cloud_server: point_cloud_server: cloud

00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 // author: Adam Leeper
00031 
00032 #include <geometry_msgs/PoseStamped.h>
00033 #include <tf/tf.h>
00034 
00035 #include "cloud_handler.h"
00036 #include <pcl_ros/transforms.h>
00037 
00038 CloudContainer::CloudContainer( ros::NodeHandle *nh, tf::TransformListener *tfl,
00039                                 std::string name, const std::string &topic) :
00040   name_(name)
00041   , topic_(topic)
00042   , nh_(nh)
00043   , tfl_(tfl)
00044 {
00045 
00046 }
00047 
00048 CloudContainer::~CloudContainer()
00049 {
00050 }
00051 
00052 void CloudContainer::clear()
00053 {
00054   cloud_ = sensor_msgs::PointCloud2();
00055 }
00056 
00057 void CloudContainer::changeFrame(const std::string &frame)
00058 {
00059   if(!frame.empty() && frame.compare(cloud_.header.frame_id))
00060   {
00061     ROS_DEBUG("Cloud in frame [%s] is being transformed to frame [%s].", 
00062               cloud_.header.frame_id.c_str(), frame.c_str());
00063     // Transform
00064     ros::Time saved_time = cloud_.header.stamp;
00065     cloud_.header.stamp = ros::Time(0);
00066     try{
00067       pcl_ros::transformPointCloud(frame, cloud_, cloud_, *tfl_);
00068     }
00069     catch(...){
00070       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00071                 frame.c_str(), cloud_.header.frame_id.c_str());
00072     }
00073     cloud_.header.stamp = saved_time;
00074   }
00075 }
00076 
00077 void CloudContainer::set( const sensor_msgs::PointCloud2 &cloud, const std::string &frame )
00078 {
00079   cloud_ = cloud;
00080   changeFrame( frame );
00081   
00082 }
00083 
00084 bool CloudContainer::get( sensor_msgs::PointCloud2 &cloud, const std::string &frame )
00085 {
00086 
00087   // This is different because we aren't changed the local cloud.
00088   if(!frame.empty() && frame.compare(cloud_.header.frame_id))
00089   {
00090     ROS_DEBUG("Cloud in frame [%s] is being returned in frame [%s].", 
00091               cloud_.header.frame_id.c_str(), frame.c_str());
00092     // Transform
00093     ros::Time saved_time = cloud_.header.stamp;
00094     cloud_.header.stamp = ros::Time(0);
00095     try{
00096       pcl_ros::transformPointCloud(frame, cloud_, cloud, *tfl_);
00097     }
00098     catch(...){
00099       ROS_ERROR("TF had a problem transforming between [%s] and [%s].",
00100                 frame.c_str(), cloud_.header.frame_id.c_str());
00101       return false;
00102     }
00103     cloud_.header.stamp = saved_time;
00104   }
00105   else
00106   {
00107     cloud = cloud_;
00108   }
00109   return true;
00110 }
00111 
00112 //sensor_msgs::PointCloud2 CloudContainer::get( const std::string &frame )
00113 //{
00114 //  return cloud_;
00115 //}
00116 
00117 bool CloudContainer::refresh(const std::string &topic, const std::string &frame)
00118 {
00119   topic_ = topic;
00120   if(topic_.empty()) return false;
00121 
00122   ROS_DEBUG("Waiting for cloud on topic [%s]", topic_.c_str());
00123   sensor_msgs::PointCloud2::ConstPtr cloud = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, *nh_, ros::Duration(10.0));
00124   if(cloud)
00125   {
00126     cloud_ = *cloud;
00127     changeFrame( frame );
00128     ROS_DEBUG("Received cloud on topic [%s] with %d points!", topic_.c_str(), cloud_.width * cloud_.height);
00129     return true;
00130   }
00131   else
00132   {
00133     this->cloud_ = sensor_msgs::PointCloud2();
00134     ROS_WARN("No message received on topic [%s]... is the remapping correct?", topic.c_str());
00135     return false;
00136   }
00137 }
00138
cloud_handler.cpp
