actionlib: actionlib: connection_monitor.cpp Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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 the Willow Garage 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 
00035 
00036 #include <actionlib/client/connection_monitor.h>
00037 
00038 #include <sstream>
00039 
00040 using namespace std;
00041 using namespace actionlib;
00042 
00043 
00044 #define CONNECTION_DEBUG(fmt, ...) \
00045     ROS_DEBUG_NAMED("ConnectionMonitor", fmt,##__VA_ARGS__)
00046 
00047 #define CONNECTION_WARN(fmt, ...) \
00048     ROS_WARN_NAMED("ConnectionMonitor", fmt,##__VA_ARGS__)
00049 
00050 
00051 ConnectionMonitor::ConnectionMonitor(ros::Subscriber&feedback_sub, ros::Subscriber& result_sub)
00052   : feedback_sub_(feedback_sub), result_sub_(result_sub)
00053 {
00054   status_received_ = false;
00055 }
00056 
00057 // ********* Goal Connections *********
00058 
00059 void ConnectionMonitor::goalConnectCallback(const ros::SingleSubscriberPublisher& pub)
00060 {
00061   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00062 
00063   // Check if it's not in the list
00064   if (goalSubscribers_.find(pub.getSubscriberName()) == goalSubscribers_.end())
00065   {
00066     CONNECTION_DEBUG("goalConnectCallback: Adding [%s] to goalSubscribers", pub.getSubscriberName().c_str());
00067     goalSubscribers_.insert(pub.getSubscriberName());
00068   }
00069   else
00070     CONNECTION_WARN("goalConnectCallback: Trying to add [%s] to goalSubscribers, but it is already in the goalSubscribers list", pub.getSubscriberName().c_str());
00071   CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
00072 
00073   check_connection_condition_.notify_all();
00074 }
00075 
00076 void ConnectionMonitor::goalDisconnectCallback(const ros::SingleSubscriberPublisher& pub)
00077 {
00078   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00079 
00080   set<string>::iterator it;
00081   it = goalSubscribers_.find(pub.getSubscriberName());
00082 
00083   if (it == goalSubscribers_.end())
00084     CONNECTION_WARN("goalDisconnectCallback: Trying to remove [%s] to goalSubscribers, but it is not in the goalSubscribers list", pub.getSubscriberName().c_str());
00085   else
00086   {
00087     CONNECTION_DEBUG("goalDisconnectCallback: Removing [%s] from goalSubscribers", pub.getSubscriberName().c_str());
00088     goalSubscribers_.erase(it);
00089   }
00090   CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
00091 }
00092 
00093 string ConnectionMonitor::goalSubscribersString()
00094 {
00095   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00096   ostringstream ss;
00097   ss << "Goal Subscribers (" << goalSubscribers_.size() << " total)";
00098   for (set<string>::iterator it=goalSubscribers_.begin(); it != goalSubscribers_.end(); it++)
00099     ss << "\n   - " << *it;
00100   return ss.str();
00101 }
00102 
00103 // ********* Cancel Connections *********
00104 
00105 void ConnectionMonitor::cancelConnectCallback(const ros::SingleSubscriberPublisher& pub)
00106 {
00107   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00108 
00109   // Check if it's not in the list
00110   if (cancelSubscribers_.find(pub.getSubscriberName()) == cancelSubscribers_.end())
00111   {
00112     CONNECTION_DEBUG("cancelConnectCallback: Adding [%s] to cancelSubscribers", pub.getSubscriberName().c_str());
00113     cancelSubscribers_.insert(pub.getSubscriberName());
00114   }
00115   else
00116     CONNECTION_WARN("cancelConnectCallback: Trying to add [%s] to cancelSubscribers, but it is already in the cancelSubscribers list", pub.getSubscriberName().c_str());
00117   CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
00118 
00119   check_connection_condition_.notify_all();
00120 }
00121 
00122 void ConnectionMonitor::cancelDisconnectCallback(const ros::SingleSubscriberPublisher& pub)
00123 {
00124   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00125 
00126   set<string>::iterator it;
00127   it = cancelSubscribers_.find(pub.getSubscriberName());
00128 
00129   if (it == cancelSubscribers_.end())
00130     CONNECTION_WARN("cancelDisconnectCallback: Trying to remove [%s] to cancelSubscribers, but it is not in the cancelSubscribers list", pub.getSubscriberName().c_str());
00131   else
00132   {
00133     CONNECTION_DEBUG("cancelDisconnectCallback: Removing [%s] from cancelSubscribers", pub.getSubscriberName().c_str());
00134     cancelSubscribers_.erase(it);
00135   }
00136   CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
00137 }
00138 
00139 string ConnectionMonitor::cancelSubscribersString()
00140 {
00141   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00142 
00143   ostringstream ss;
00144   ss << "cancel Subscribers (" << cancelSubscribers_.size() << " total)";
00145   for (set<string>::iterator it=cancelSubscribers_.begin(); it != cancelSubscribers_.end(); it++)
00146     ss << "\n   - " << *it;
00147   return ss.str();
00148 }
00149 
00150 // ********* GoalStatus Connections *********
00151 void ConnectionMonitor::processStatus(const actionlib_msgs::GoalStatusArrayConstPtr& status)
00152 {
00153   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00154 
00155   string cur_status_caller_id = (*(status->__connection_header))["callerid"];
00156 
00157   if (status_received_)
00158   {
00159     if (status_caller_id_ != cur_status_caller_id)
00160     {
00161       CONNECTION_WARN("processStatus: Previously received status from [%s], but we now received status from [%s]. Did the ActionServer change?",
00162                    status_caller_id_.c_str(), cur_status_caller_id.c_str());
00163       status_caller_id_ = cur_status_caller_id;
00164     }
00165     latest_status_time_ = status->header.stamp;
00166   }
00167   else
00168   {
00169     CONNECTION_DEBUG("processStatus: Just got our first status message from the ActionServer at node [%s]", cur_status_caller_id.c_str());
00170     status_received_ = true;
00171     status_caller_id_ = cur_status_caller_id;
00172     latest_status_time_ = status->header.stamp;
00173   }
00174 
00175   check_connection_condition_.notify_all();
00176 }
00177 
00178 // ********* Connection logic *********
00179 bool ConnectionMonitor::isServerConnected()
00180 {
00181   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00182 
00183   if (!status_received_)
00184   {
00185     CONNECTION_DEBUG("isServerConnected: Didn't receive status yet, so not connected yet");
00186     return false;
00187   }
00188 
00189   if(goalSubscribers_.find(status_caller_id_) == goalSubscribers_.end())
00190   {
00191     CONNECTION_DEBUG("isServerConnected: Server [%s] has not yet subscribed to the goal topic, so not connected yet", status_caller_id_.c_str());
00192     CONNECTION_DEBUG("%s", goalSubscribersString().c_str());
00193     return false;
00194   }
00195 
00196   if(cancelSubscribers_.find(status_caller_id_) == cancelSubscribers_.end())
00197   {
00198     CONNECTION_DEBUG("isServerConnected: Server [%s] has not yet subscribed to the cancel topic, so not connected yet", status_caller_id_.c_str());
00199     CONNECTION_DEBUG("%s", cancelSubscribersString().c_str());
00200     return false;
00201   }
00202 
00203   if(feedback_sub_.getNumPublishers() == 0)
00204   {
00205     CONNECTION_DEBUG("isServerConnected: Client has not yet connected to feedback topic of server [%s]", status_caller_id_.c_str());
00206     return false;
00207   }
00208 
00209   if(result_sub_.getNumPublishers() == 0)
00210   {
00211     CONNECTION_DEBUG("isServerConnected: Client has not yet connected to result topic of server [%s]", status_caller_id_.c_str());
00212     return false;
00213   }
00214 
00215   CONNECTION_DEBUG("isServerConnected: Server [%s] is fully connected", status_caller_id_.c_str());
00216   return true;
00217 }
00218 
00219 bool ConnectionMonitor::waitForActionServerToStart(const ros::Duration& timeout, const ros::NodeHandle& nh)
00220 {
00221   if (timeout < ros::Duration(0,0))
00222     ROS_ERROR_NAMED("actionlib", "Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
00223 
00224   ros::Time timeout_time = ros::Time::now() + timeout;
00225 
00226   boost::recursive_mutex::scoped_lock lock(data_mutex_);
00227 
00228   if (isServerConnected())
00229     return true;
00230 
00231   // Hardcode how often we check for node.ok()
00232   ros::Duration loop_period = ros::Duration().fromSec(.5);
00233 
00234   while (nh.ok() && !isServerConnected())
00235   {
00236     // Determine how long we should wait
00237     ros::Duration time_left = timeout_time - ros::Time::now();
00238 
00239     // Check if we're past the timeout time
00240     if (timeout != ros::Duration(0,0) && time_left <= ros::Duration(0,0) )
00241       break;
00242 
00243     // Truncate the time left
00244     if (time_left > loop_period || timeout == ros::Duration())
00245       time_left = loop_period;
00246 
00247     check_connection_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
00248   }
00249 
00250   return isServerConnected();
00251 }
00252 
00253 
00254 
00255 
00256 
00257 
00258 
00259