Using helper function for waiting for ActionServer (#192)

personalrobotics · Apr 26, 2017 · 797797f · 797797f
1 parent dffdf28
commit 797797f
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 27 deletions.
diff --git a/include/aikido/control/ros/RosTrajectoryExecutor.hpp b/include/aikido/control/ros/RosTrajectoryExecutor.hpp
@@ -60,8 +60,6 @@ class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
     = actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction>;
   using GoalHandle = TrajectoryActionClient::GoalHandle;
 
-  bool waitForServer();
-
   void transitionCallback(GoalHandle handle);
 
   ::ros::NodeHandle mNode;

diff --git a/src/control/ros/RosTrajectoryExecutor.cpp b/src/control/ros/RosTrajectoryExecutor.cpp
@@ -1,6 +1,7 @@
 #include <aikido/control/ros/Conversions.hpp>
 #include <aikido/control/ros/RosTrajectoryExecutor.hpp>
 #include <aikido/control/ros/RosTrajectoryExecutionException.hpp>
+#include <aikido/control/ros/util.hpp>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 #include <aikido/statespace/dart/RnJoint.hpp>
 #include <aikido/statespace/dart/SO2Joint.hpp>
@@ -114,7 +115,13 @@ std::future<void> RosTrajectoryExecutor::execute(
   goal.trajectory = toRosJointTrajectory(
     traj, mTimestep);
 
-  if (!waitForServer())
+  bool waitForServer = waitForActionServer<
+                        control_msgs::FollowJointTrajectoryAction,
+                        std::chrono::milliseconds,
+                        std::chrono::milliseconds>
+                        (mClient, mCallbackQueue, mConnectionTimeout, mConnectionPollingPeriod);
+
+  if (!waitForServer)
     throw std::runtime_error("Unable to connect to action server.");
 
   {
@@ -133,30 +140,6 @@ std::future<void> RosTrajectoryExecutor::execute(
   }
 }
 
-//=============================================================================
-bool RosTrajectoryExecutor::waitForServer()
-{
-  using Clock = std::chrono::steady_clock;
-
-  const auto startTime = Clock::now();
-  const auto endTime = startTime + mConnectionTimeout;
-  auto currentTime = startTime + mConnectionPollingPeriod;
-
-  while (currentTime < endTime)
-  {
-    mCallbackQueue.callAvailable();
-
-    // TODO: Is this thread safe?
-    if (mClient.isServerConnected())
-      return true;
-
-    currentTime += mConnectionPollingPeriod;
-    std::this_thread::sleep_until(currentTime);
-  }
-
-  return false;
-}
-
 //=============================================================================
 void RosTrajectoryExecutor::transitionCallback(GoalHandle handle)
 {