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 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00033 
00034 #include <ros/ros.h>
00035 #include <math.h>
00036 
00037 #include <boost/thread.hpp>
00038 #include <tf/transform_listener.h>
00039 
00040 #include <actionlib/server/simple_action_server.h>
00041 
00042 #include "pcl_ros/transforms.h"
00043 
00044 #include "cloud_handler.h"
00045 #include <point_cloud_server/StoreCloudAction.h>
00046 
00049 class CloudServer
00050 {
00051 public:
00052 
00053   CloudServer() :
00054       nh_("/"),
00055       pnh_("~"),
00056       store_cloud_name_(ros::this_node::getName()),
00057       store_cloud_server_(nh_, store_cloud_name_, boost::bind(&CloudServer::storeCloud, this, _1), false)
00058   {
00059     ROS_INFO("%s is running.", store_cloud_name_.c_str() );
00060     store_cloud_server_.start();
00061   }
00062 
00063   ~CloudServer()
00064   {
00065   }
00066 
00067 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00068 protected:
00069 
00071   void handleTopicError(const point_cloud_server::StoreCloudGoalConstPtr &goal, const std::string &resolved_topic)
00072   {
00073     point_cloud_server::StoreCloudResult result;
00074     std::stringstream ss;
00075     ss << "Failed to get cloud [" << goal->name << "] on topic [" << resolved_topic << "]. Deleting from server.";
00076     ROS_WARN_STREAM(ss);
00077     cloud_map_.erase(cloud_map_it_);
00078     result.result = result.TOPIC_ERROR;
00079     store_cloud_server_.setAborted(result, ss.str());
00080   }
00081 
00083   void handleMissingEntryError(const point_cloud_server::StoreCloudGoalConstPtr &goal)
00084   {
00085     point_cloud_server::StoreCloudResult result;
00086     std::stringstream ss;
00087     ss << "Didn't find cloud [" << goal->name << "] on cloud server!"
00088         << "(If you were trying to get a cloud, did you forget to provide a topic?)";
00089     ROS_WARN_STREAM(ss);
00090     result.result = result.NAME_ERROR;
00091     store_cloud_server_.setAborted(result, ss.str() );
00092   }
00093 
00095   void handleTFError(const point_cloud_server::StoreCloudGoalConstPtr &goal)
00096   {
00097     point_cloud_server::StoreCloudResult result;
00098     std::stringstream ss;
00099     ss << "There was an error applying a transform.";
00100     ROS_WARN_STREAM(ss);
00101     result.result = result.TF_ERROR;
00102     store_cloud_server_.setAborted(result, ss.str() );
00103   }
00104 
00106   void handleOtherError(const std::string &text)
00107   {
00108     point_cloud_server::StoreCloudResult result;
00109     ROS_ERROR_STREAM(text);
00110     result.result = result.OTHER_ERROR;
00111     store_cloud_server_.setAborted(result, text);
00112     return;
00113   }
00114 
00115 
00117   void storeCloud(const point_cloud_server::StoreCloudGoalConstPtr &goal)
00118   {
00119     std::string resolved_topic = nh_.resolveName(goal->topic, true);
00120 
00121     if(goal->name.empty())
00122     {
00123       handleOtherError("You must provide either a topic or a non-empty point cloud when specifying a STORE action.");
00124       return;
00125     }
00126     
00127     ROS_DEBUG(  "Received goal with name [%s], topic [%s] resolved to [%s], and action %d.", 
00128                 goal->name.c_str(), goal->topic.c_str(), resolved_topic.c_str(), goal->action);
00129     
00130     point_cloud_server::StoreCloudResult result;
00131 
00132 //    if(goal->action == goal->STORE || goal->action == goal->GET)
00133 //    {
00134 //      ROS_DEBUG("Cloud server is doing a STORE/GET action");
00135 //
00136 //      bool cloud_exists = goal->cloud.width*goal->cloud.height > 0;
00137 //
00138 //
00139 //      // First check if we have either a topic or a point cloud to use...
00140 //      if(goal->action == goal->STORE && goal->topic.empty() && !cloud_exists)
00141 //      {
00142 //        handleOtherError("You must provide either a topic or a non-empty point cloud when specifying a STORE action.");
00143 //        return;
00144 //      }
00145 //
00146 //      // Check if named handler exists, and if not, add it to the map:
00147 //      cloud_map_it_ = cloud_map_.find(goal->name);
00148 //      if(cloud_map_it_ == cloud_map_.end())
00149 //      {
00150 //        // Return error if we called GET without a topic or cloud
00151 //        if(goal->action == goal->GET && goal->topic.empty() && !cloud_exists)
00152 //        {
00153 //          handleMissingEntryError(goal);
00154 //          return;
00155 //        }
00156 //        else
00157 //        {
00158 //          ROS_DEBUG("Adding new entry [%s] to cloud server.", goal->name.c_str());
00159 //          cloud_map_it_ = cloud_map_.insert(
00160 //              std::pair<std::string, boost::shared_ptr<CloudContainer> >(
00161 //                  goal->name, boost::shared_ptr<CloudContainer>(new CloudContainer(&nh_, goal->name, resolved_topic)))
00162 //              ).first;
00163 //        }
00164 //      }
00165 //
00166 //      // Now that we have a handler...
00167 //      if( goal->action == goal->STORE )
00168 //      {
00169 //        if(goal->topic.empty() )
00170 //        {
00171 //          cloud_map_it_->second->set( goal->cloud, goal->storage_frame_id );
00172 //          ROS_DEBUG("Stored the cloud provided in the action goal into entry [%s].", goal->name.c_str());
00173 //        }
00174 //        else
00175 //        {
00176 //          if( !cloud_map_it_->second->refresh( resolved_topic, goal->storage_frame_id ) )
00177 //          {
00178 //            handleTopicError(goal, resolved_topic);
00179 //            return;
00180 //          }
00181 //          ROS_DEBUG("Stored cloud [%s] from topic [%s].",
00182 //                    goal->name.c_str(), resolved_topic.c_str());
00183 //        }
00184 //        store_cloud_server_.setSucceeded(result);
00185 //        return;
00186 //      }
00187 //    }
00188 
00189 
00190 
00191 
00192     if(goal->action == goal->STORE)
00193     {
00194       ROS_DEBUG("Cloud server is doing a STORE/GET action");
00195 
00196       // First check if we have either a topic or a point cloud to use...
00197       if(goal->topic.empty() && (goal->cloud.width*goal->cloud.height == 0))
00198       {
00199         std::stringstream ss;
00200         ss << "You are incorrectly using the StoreCloud API "
00201            << "You must provide either a topic or a non-empty point cloud when specifying a STORE action.";
00202         ROS_ERROR_STREAM(ss);
00203         store_cloud_server_.setAborted(result, ss.str());
00204         return;
00205       }
00206 
00207       // Check if named handler exists, and if not, add it to the map:
00208       cloud_map_it_ = cloud_map_.find(goal->name);
00209       if(cloud_map_it_ == cloud_map_.end())
00210       {
00211         ROS_DEBUG("Adding new cloud [%s] to cloud server.", goal->name.c_str());
00212         cloud_map_it_ = cloud_map_.insert(
00213             std::pair<std::string, boost::shared_ptr<CloudContainer> >(
00214                 goal->name, boost::shared_ptr<CloudContainer>(
00215                     new CloudContainer(&nh_, &tfl_, goal->name, resolved_topic)))).first;
00216       }
00217 
00218       if(goal->topic.empty())
00219       {
00220         cloud_map_it_->second->set( goal->cloud, goal->storage_frame_id );
00221         ROS_DEBUG("Stored the cloud provided in the action goal into cloud [%s].", goal->name.c_str());
00222       }
00223       else
00224       {
00225         if( !cloud_map_it_->second->refresh( resolved_topic, goal->storage_frame_id ) )
00226         {
00227           handleTopicError(goal, resolved_topic);
00228           return;
00229         }
00230         ROS_DEBUG("Stored cloud [%s] from topic [%s].",
00231                   goal->name.c_str(), resolved_topic.c_str());
00232       }
00233       store_cloud_server_.setSucceeded(result);
00234       return;
00235     }
00236     else if(goal->action == goal->GET)
00237     {
00238       ROS_DEBUG("Cloud server is doing a GET action");
00239       cloud_map_it_ = cloud_map_.find(goal->name);
00240       if(cloud_map_it_ != cloud_map_.end() || !goal->topic.empty())
00241       {
00242         if(goal->topic.empty())
00243         {
00244           ROS_DEBUG("Fetching cloud [%s] from cloud server.", goal->name.c_str());
00245         }
00246         else if(cloud_map_it_ == cloud_map_.end())
00247         {
00248           ROS_DEBUG("Storing new cloud [%s] on cloud server.", goal->name.c_str());
00249           cloud_map_it_ = cloud_map_.insert(
00250               std::pair<std::string, boost::shared_ptr<CloudContainer> >(
00251                   goal->name, boost::shared_ptr<CloudContainer>(
00252                       new CloudContainer(&nh_, &tfl_, goal->name, resolved_topic)))).first;
00253           if( !cloud_map_it_->second->refresh( resolved_topic, goal->storage_frame_id ) )
00254           {
00255             handleTopicError(goal, resolved_topic);
00256             return;
00257           }
00258         }
00259         else
00260         {
00261           ROS_DEBUG("Refreshing cloud [%s] on cloud server.", goal->name.c_str());
00262           if( !cloud_map_it_->second->refresh( resolved_topic, goal->storage_frame_id ) )
00263           {
00264             handleTopicError(goal, resolved_topic);
00265             return;
00266           }
00267         }
00268 
00269         // Actually get the cloud
00270         ROS_DEBUG("Getting cloud [%s] on cloud server and returning with result.", goal->name.c_str());
00271         if(!cloud_map_it_->second->get( result.cloud, goal->result_frame_id ))
00272         {
00273           handleTFError(goal);
00274           return;
00275         }
00276         store_cloud_server_.setSucceeded(result);
00277         return;
00278       }
00279       else
00280       {
00281         handleMissingEntryError(goal);
00282         return;
00283       }
00284     }
00285     else if(goal->action == goal->CLEAR)
00286     {
00287       ROS_DEBUG("Cloud server is doing a CLEAR action.");
00288       cloud_map_it_ = cloud_map_.find(goal->name);
00289       if(cloud_map_it_ != cloud_map_.end())
00290       {
00291         ROS_DEBUG("Removing cloud [%s] from cloud server.", goal->name.c_str());
00292         cloud_map_.erase(cloud_map_it_);
00293         store_cloud_server_.setSucceeded(result);
00294       }
00295       else
00296       {
00297         handleMissingEntryError(goal);
00298         return;
00299       }
00300     }
00301   }
00302 
00303 // **** 10 ***** 20 ****** 30 ****** 40 ****** 50 ****** 60 ****** 70 ****** 80 ****** 90 ***** 100 ***** 110 ***** 120
00304 
00306   ros::NodeHandle nh_;
00307   ros::NodeHandle pnh_;
00308 
00310   tf::TransformListener tfl_;
00311 
00313   std::map< std::string, boost::shared_ptr<CloudContainer> > cloud_map_;
00314 
00316   std::map< std::string, boost::shared_ptr<CloudContainer> >::iterator cloud_map_it_;
00317 
00319   std::string store_cloud_name_;
00320 
00322   actionlib::SimpleActionServer<point_cloud_server::StoreCloudAction> store_cloud_server_;
00323 };
00324 
00325 
00326 int main(int argc, char** argv)
00327 {
00328   ros::init(argc, argv, "cloud_server_action");
00329   CloudServer cloud_server;
00330   ros::spin();
00331 }
cloud_server.cpp
