use waitForCurrentState(ros::Time::now(), instead of waitForCurrentSt…

…ate(1.0) in TrajectoryExecutionManager::validate, cherry pick part of moveit#350

moveit_ros/planning/CMakeLists.txt

@@ -5,7 +5,7 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
 
-find_package(Boost REQUIRED system filesystem date_time program_options signals thread)
+find_package(Boost REQUIRED system filesystem date_time program_options signals thread chrono)
 find_package(catkin REQUIRED COMPONENTS
   moveit_core
   moveit_ros_perception

moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h

@@ -44,6 +44,7 @@
 #include <boost/function.hpp>
 #include <boost/shared_ptr.hpp>
 #include <boost/thread/mutex.hpp>
+#include <boost/thread/condition_variable.hpp>
 
 namespace planning_scene_monitor
 {
@@ -134,6 +135,11 @@ class CurrentStateMonitor
    *  @return Returns the map from joint names to joint state values*/
   std::map<std::string, double> getCurrentStateValues() const;
 
+  /** @brief Wait for at most \e wait_time seconds (default 1s) for a robot state more recent than t
+   *  @return true on success, false if up-to-date robot state wasn't received within \e wait_time
+   */
+  bool waitForCurrentState(const ros::Time t = ros::Time::now(), double wait_time = 1.0) const;
+
   /** @brief Wait for at most \e wait_time seconds until the complete current state is known. Return true if the full
    * state is known */
   bool waitForCurrentState(double wait_time) const;
@@ -189,6 +195,7 @@ class CurrentStateMonitor
   ros::Time last_tf_update_;
 
   mutable boost::mutex state_update_lock_;
+  mutable boost::condition_variable state_update_condition_;
   std::vector<JointStateUpdateCallback> update_callbacks_;
 };
 

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -261,6 +261,23 @@ bool planning_scene_monitor::CurrentStateMonitor::haveCompleteState(const ros::D
   return result;
 }
 
+bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_time) const
+{
+  ros::WallTime start = ros::WallTime::now();
+  ros::WallDuration elapsed(0, 0);
+  ros::WallDuration timeout(wait_time);
+
+  boost::mutex::scoped_lock lock(state_update_lock_);
+  while (current_state_time_ < t)
+  {
+    state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
+    elapsed = ros::WallTime::now() - start;
+    if (elapsed > timeout)
+      return false;
+  }
+  return true;
+}
+
 bool planning_scene_monitor::CurrentStateMonitor::waitForCurrentState(double wait_time) const
 {
   double slept_time = 0.0;
@@ -394,4 +411,8 @@ void planning_scene_monitor::CurrentStateMonitor::jointStateCallback(const senso
   if (update)
     for (std::size_t i = 0; i < update_callbacks_.size(); ++i)
       update_callbacks_[i](joint_state);
+
+
+  // notify waitForCurrentState() *after* potential update callbacks
+  state_update_condition_.notify_all();
 }

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -932,7 +932,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont
   ROS_DEBUG_NAMED("traj_execution", "Validating trajectory with allowed_start_tolerance %g", allowed_start_tolerance_);
 
   robot_state::RobotStatePtr current_state;
-  if (!csm_->waitForCurrentState(1.0) || !(current_state = csm_->getCurrentState()))
+  if (!csm_->waitForCurrentState(ros::Time::now()) || !(current_state = csm_->getCurrentState()))
   {
     ROS_WARN_NAMED("traj_execution", "Failed to validate trajectory: couldn't receive full current joint state within "
                                      "1s");