[Personalrobots-commit] SF.net SVN: personalrobots:[21770] pkg/trunk

[<vij...@us...>]

Revision: 21770
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=21770&view=rev
Author:   vijaypradeep
Date:     2009-08-13 04:45:31 +0000 (Thu, 13 Aug 2009)

Log Message:
-----------
Adding waitForActionServerToStart call

Modified Paths:
--------------
    pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
    pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
    pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h

Modified: pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp
===================================================================
--- pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp	2009-08-13 04:45:08 UTC (rev 21769)
+++ pkg/trunk/sandbox/move_base_client/src/move_base_client.cpp	2009-08-13 04:45:31 UTC (rev 21770)
@@ -62,9 +62,15 @@
   ROS_INFO("Got Feedback!");
 }
 
-void spinThread()
+
+void spinThread(ros::NodeHandle* n)
 {
-  ros::spin();
+  // I can't figure out how to cleanly exit ros::spin(), so I made this hack instead
+  while(n->ok())
+  {
+    ros::spinOnce();
+    usleep(10);
+  }
 }
 
 int main(int argc, char** argv)
@@ -73,19 +79,26 @@
 
   ros::NodeHandle n;
 
-  boost::thread spinthread = boost::thread(boost::bind(&spinThread)) ;
+  boost::thread spinthread = boost::thread(boost::bind(&spinThread, &n)) ;
 
   MoveBaseClient ac("move_base");
 
-  //ros::Duration sleep_duration = ros::Duration().fromSec(1.0);
-  //sleep_duration.sleep();
-  sleep(2.0);
+  ROS_INFO("Waiting for action server to start");
+  if (ac.waitForActionServerToStart( ros::Duration(10,0)))
+    ROS_INFO("Connected to action server");
+  else
+  {
+    ROS_ERROR("Timed out waiting for action server");
+    n.shutdown();
+    spinthread.join();
+    return 0;
+  }
 
   MoveBaseGoal goal;
 
   ActionClient<MoveBaseAction>::GoalHandle gh = ac.sendGoal(goal, &transitionCallback, &feedbackCallback);
 
-  sleep(1.0);
+  sleep(3.0);
 
   gh.cancel();
 
@@ -105,8 +118,8 @@
   while(n.ok())
     sleep(.1);
 
-  sleep(3);
+  n.shutdown();
+  spinthread.join();
 
-
   return 0;
 }

Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h	2009-08-13 04:45:08 UTC (rev 21769)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/action_client.h	2009-08-13 04:45:31 UTC (rev 21770)
@@ -35,6 +35,8 @@
 #ifndef ACTIONLIB_ACTION_CLIENT_H_
 #define ACTIONLIB_ACTION_CLIENT_H_
 
+#include <boost/thread/condition.hpp>
+
 #include "ros/ros.h"
 #include "actionlib/client/client_helpers.h"
 
@@ -129,6 +131,46 @@
     goal_pub_.publish(cancel_msg);
   }
 
+  /**
+   * \brief Waits for the ActionServer to connect to this client
+   * Often, it can take a second for the action server & client to negotiate
+   * a connection, thus, risking the first few goals to be dropped. This call lets
+   * the user wait until the network connection to the server is negotiated
+   */
+  bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) )
+  {
+    if (timeout < ros::Duration(0,0))
+      ROS_WARN("Timeouts can't be negative. Timeout is [%.2fs]", timeout.toSec());
+
+    ros::Time timeout_time = ros::Time::now() + timeout;
+
+    boost::mutex::scoped_lock lock(server_connection_mutex_);
+
+    if (server_connected_)
+      return true;
+
+    // Hardcode how often we check for node.ok()
+    ros::Duration loop_period = ros::Duration().fromSec(.1);
+
+    while (n_.ok() && !server_connected_)
+    {
+      // Determine how long we should wait
+      ros::Duration time_left = timeout_time - ros::Time::now();
+
+      // Check if we're past the timeout time
+      if (timeout != ros::Duration(0,0) && time_left <= ros::Duration(0,0) )
+        break;
+
+      // Truncate the time left
+      if (time_left > loop_period)
+        time_left = loop_period;
+
+      server_connection_condition_.timed_wait(lock, boost::posix_time::milliseconds(time_left.toSec() * 1000.0f));
+    }
+
+    return server_connected_;
+  }
+
 private:
   ros::NodeHandle n_;
 
@@ -140,6 +182,10 @@
 
   GoalManager<ActionSpec> manager_;
 
+  boost::mutex server_connection_mutex_;
+  boost::condition server_connection_condition_;
+  bool server_connected_;
+
   void sendGoalFunc(const ActionGoalConstPtr& action_goal)
   {
     goal_pub_.publish(action_goal);
@@ -153,7 +199,8 @@
   void initClient()
   {
     // Start publishers and subscribers
-    goal_pub_ = n_.advertise<ActionGoal>("goal", 1, true);
+    server_connected_ = false;
+    goal_pub_ = n_.advertise<ActionGoal>("goal", 1, boost::bind(&ActionClient::serverConnectionCb, this, _1));
     cancel_pub_ = n_.advertise<GoalID>("cancel", 1, true);
     manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
     manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
@@ -177,6 +224,13 @@
   {
     manager_.updateResults(action_result);
   }
+
+  void serverConnectionCb(const ros::SingleSubscriberPublisher& pub)
+  {
+    boost::mutex::scoped_lock lock(server_connection_mutex_);
+    server_connected_ = true;
+    server_connection_condition_.notify_all();
+  }
 };
 
 

Modified: pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h
===================================================================
--- pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h	2009-08-13 04:45:08 UTC (rev 21769)
+++ pkg/trunk/stacks/common/actionlib/include/actionlib/client/simple_action_client.h	2009-08-13 04:45:31 UTC (rev 21770)
@@ -91,6 +91,13 @@
   }
 
   /**
+   * \brief Blocks until the action server connects to this client
+   * \param timeout Max time to block before returning. A zero timeout is interpreted as an infinite timeout.
+   * \return True if the server connected in the allocated time. False on timeout
+   */
+  bool waitForActionServerToStart(const ros::Duration& timeout = ros::Duration(0,0) ) { return ac_.waitForActionServerToStart(timeout); }
+
+  /**
    * \brief Sends a goal to the ActionServer, and also registers callbacks
    *
    * If a previous goal is already active when this is called. We simply forget


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.