tf2_ros: tf2_ros: buffer_client.cpp Source File

00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <tf2_ros/buffer_client.h>
00038 
00039 namespace tf2
00040 {
00041   BufferClient::BufferClient(std::string ns, double check_frequency, ros::Duration timeout_padding): 
00042     client_(ns), 
00043     check_frequency_(check_frequency),
00044     timeout_padding_(timeout_padding)
00045   {
00046   }
00047 
00048   geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const std::string& source_frame,
00049       const ros::Time& time, const ros::Duration timeout) const
00050   {
00051     //populate the goal message
00052     tf2_msgs::LookupTransformGoal goal;
00053     goal.target_frame = target_frame;
00054     goal.source_frame = source_frame;
00055     goal.source_time = time;
00056     goal.timeout = timeout;
00057     goal.advanced = false;
00058 
00059     return processGoal(goal);
00060   }
00061 
00062   geometry_msgs::TransformStamped BufferClient::lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00063       const std::string& source_frame, const ros::Time& source_time,
00064       const std::string& fixed_frame, const ros::Duration timeout) const
00065   {
00066     //populate the goal message
00067     tf2_msgs::LookupTransformGoal goal;
00068     goal.target_frame = target_frame;
00069     goal.source_frame = source_frame;
00070     goal.source_time = source_time;
00071     goal.timeout = timeout;
00072     goal.target_time = target_time;
00073     goal.fixed_frame = fixed_frame;
00074     goal.advanced = true;
00075 
00076     return processGoal(goal);
00077   }
00078 
00079   geometry_msgs::TransformStamped BufferClient::processGoal(const tf2_msgs::LookupTransformGoal& goal) const
00080   {
00081     client_.sendGoal(goal);
00082     ros::Rate r(check_frequency_);
00083     bool timed_out = false;
00084     ros::Time start_time = ros::Time::now();
00085     while(ros::ok() && !client_.getState().isDone() && !timed_out)
00086     {
00087       timed_out = ros::Time::now() > start_time + goal.timeout + timeout_padding_;
00088       r.sleep();
00089     }
00090 
00091     //this shouldn't happen, but could in rare cases where the server hangs
00092     if(timed_out)
00093     {
00094       //make sure to cancel the goal the server is pursuing
00095       client_.cancelGoal();
00096       throw TimeoutException("The LookupTransform goal sent to the BufferServer did not come back in the specified time. Something is likely wrong with the server.");
00097     }
00098 
00099     if(client_.getState() != actionlib::SimpleClientGoalState::SUCCEEDED)
00100       throw TimeoutException("The LookupTransform goal sent to the BufferServer did not come back with SUCCEEDED status. Something is likely wrong with the server.");
00101 
00102     //process the result for errors and return it
00103     return processResult(*client_.getResult());
00104   }
00105 
00106   geometry_msgs::TransformStamped BufferClient::processResult(const tf2_msgs::LookupTransformResult& result) const
00107   {
00108     //if there's no error, then we'll just return the transform
00109     if(result.error.error != result.error.NO_ERROR){
00110       //otherwise, we'll have to throw the appropriate exception
00111       if(result.error.error == result.error.LOOKUP_ERROR)
00112         throw LookupException(result.error.error_string);
00113 
00114       if(result.error.error == result.error.CONNECTIVITY_ERROR)
00115         throw ConnectivityException(result.error.error_string);
00116 
00117       if(result.error.error == result.error.EXTRAPOLATION_ERROR)
00118         throw ExtrapolationException(result.error.error_string);
00119 
00120       if(result.error.error == result.error.INVALID_ARGUMENT_ERROR)
00121         throw InvalidArgumentException(result.error.error_string);
00122 
00123       if(result.error.error == result.error.TIMEOUT_ERROR)
00124         throw TimeoutException(result.error.error_string);
00125 
00126       throw TransformException(result.error.error_string);
00127     }
00128 
00129     return result.transform;
00130   }
00131 
00132   bool BufferClient::canTransform(const std::string& target_frame, const std::string& source_frame, 
00133         const ros::Time& time, const ros::Duration timeout, std::string* errstr) const
00134   {
00135     try
00136     {
00137       lookupTransform(target_frame, source_frame, time, timeout);
00138       return true;
00139     }
00140     catch(tf2::TransformException& ex)
00141     {
00142       if(errstr)
00143         *errstr = ex.what();
00144       return false;
00145     }
00146   }
00147 
00148   bool BufferClient::canTransform(const std::string& target_frame, const ros::Time& target_time,
00149         const std::string& source_frame, const ros::Time& source_time,
00150         const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr) const
00151   {
00152     try
00153     {
00154       lookupTransform(target_frame, target_time, source_frame, source_time, fixed_frame, timeout);
00155       return true;
00156     }
00157     catch(tf2::TransformException& ex)
00158     {
00159       if(errstr)
00160         *errstr = ex.what();
00161       return false;
00162     }
00163   }
00164 };