Allow waiting for joint model group states and retrieval of group-spe…

…cific timestamps (moveit#3580)

This commit introduces:

1. per-group current state time retrieval
2. per-group complete robot state waiting

This fixes the following issue: 
I want to call waitForCompleteState on group_A, which the API already supports. There is also another joint out_of_group_A in the robot model that is not available until some time after bringup. 
I'm calling waitForCompleteState(group_A, <timeout>) way earlier than out_of_group_A is published by its source.
The problem with the current implementation is that it waits until <timeout> elapses, but returns true since all joints of group_A exist. I don't see a point in waiting for an out-of-group joint in this case.

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

@@ -96,57 +96,58 @@ class CurrentStateMonitor
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @return False if we have no joint state information for one or more of the joints
    */
-  inline bool haveCompleteState() const
+  inline bool haveCompleteState(const std::string& group = "") const
   {
-    return haveCompleteStateHelper(ros::Time(0), nullptr);
+    return haveCompleteStateHelper(ros::Time(0), nullptr, group);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @param oldest_allowed_update_time All joint information must be from this time or more current
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old*/
-  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time) const
+  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time, const std::string& group = "") const
   {
-    return haveCompleteStateHelper(oldest_allowed_update_time, nullptr);
+    return haveCompleteStateHelper(oldest_allowed_update_time, nullptr, group);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @param age Joint information must be at most this old
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old
    */
-  inline bool haveCompleteState(const ros::Duration& age) const
+  inline bool haveCompleteState(const ros::Duration& age, const std::string& group = "") const
   {
-    return haveCompleteStateHelper(ros::Time::now() - age, nullptr);
+    return haveCompleteStateHelper(ros::Time::now() - age, nullptr, group);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @param missing_joints Returns the list of joints that are missing
    *  @return False if we have no joint state information for one or more of the joints
    */
-  inline bool haveCompleteState(std::vector<std::string>& missing_joints) const
+  inline bool haveCompleteState(std::vector<std::string>& missing_joints, const std::string& group = "") const
   {
-    return haveCompleteStateHelper(ros::Time(0), &missing_joints);
+    return haveCompleteStateHelper(ros::Time(0), &missing_joints, group);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @param oldest_allowed_update_time All joint information must be from this time or more current
    *  @param missing_joints Returns the list of joints that are missing
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old*/
-  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time,
-                                std::vector<std::string>& missing_joints) const
+  inline bool haveCompleteState(const ros::Time& oldest_allowed_update_time, std::vector<std::string>& missing_joints,
+                                const std::string& group = "") const
   {
-    return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints);
+    return haveCompleteStateHelper(oldest_allowed_update_time, &missing_joints, group);
   }
 
   /** @brief Query whether we have joint state information for all DOFs in the kinematic model
    *  @return False if we have no joint state information for one of the joints or if our state
    *  information is more than \e age old
    */
-  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints) const
+  inline bool haveCompleteState(const ros::Duration& age, std::vector<std::string>& missing_joints,
+                                const std::string& group = "") const
   {
-    return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints);
+    return haveCompleteStateHelper(ros::Time::now() - age, &missing_joints, group);
   }
 
   /** @brief Get the current state
@@ -156,12 +157,21 @@ class CurrentStateMonitor
   /** @brief Set the state \e upd to the current state maintained by this class. */
   void setToCurrentState(moveit::core::RobotState& upd) const;
 
-  /** @brief Get the time stamp for the current state */
-  ros::Time getCurrentStateTime() const;
+  /** @brief Get the time stamp for the current state
+   *  @param group The name of the joint group whose current state is asked, defaults to empty string, which means all
+   * the active joints.
+   *  @note If there are multiple joint sources, then the oldest joint's timestamp is retrieved for the particular group
+   *  @return The current state timestamp related with group
+   */
+  ros::Time getCurrentStateTime(const std::string& group = "") const;
 
   /** @brief Get the current state and its time stamp
+   *  @param group The name of the joint group whose current state is asked, defaults to empty string, which means all
+   * the active joints.
+   *  @note If there are multiple sources of the constituent joints, then the oldest joint's timestamp is retrieved for
+   * the particular group
    *  @return Returns a pair of the current state and its time stamp */
-  std::pair<moveit::core::RobotStatePtr, ros::Time> getCurrentStateAndTime() const;
+  std::pair<moveit::core::RobotStatePtr, ros::Time> getCurrentStateAndTime(const std::string& group = "") const;
 
   /** @brief Get the current state values as a map from joint names to joint state values
    *  @return Returns the map from joint names to joint state values*/
@@ -219,8 +229,12 @@ class CurrentStateMonitor
   }
 
 private:
-  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
-                               std::vector<std::string>* missing_joints) const;
+  /**
+   * Lock-free method that is used by @ref getCurrentStateTime and @ref getCurrentStateAndTime methods.
+   */
+  ros::Time getCurrentStateTimeHelper(const std::string& group = "") const;
+  bool haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time, std::vector<std::string>* missing_joints,
+                               const std::string& group) const;
 
   void jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state);
   void tfCallback();
@@ -235,7 +249,6 @@ class CurrentStateMonitor
   ros::Time monitor_start_time_;
   double error_;
   ros::Subscriber joint_state_subscriber_;
-  ros::Time current_state_time_;
 
   mutable boost::mutex state_update_lock_;
   mutable boost::condition_variable state_update_condition_;

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

@@ -36,11 +36,11 @@
 
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
 
+#include <boost/algorithm/string/join.hpp>
+#include <limits>
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
-#include <limits>
-
 constexpr char LOGNAME[] = "current_state_monitor";
 
 namespace planning_scene_monitor
@@ -76,17 +76,57 @@ moveit::core::RobotStatePtr CurrentStateMonitor::getCurrentState() const
   return moveit::core::RobotStatePtr(result);
 }
 
-ros::Time CurrentStateMonitor::getCurrentStateTime() const
+ros::Time CurrentStateMonitor::getCurrentStateTimeHelper(const std::string& group) const
+{
+  const std::vector<const moveit::core::JointModel*>* active_joints = &robot_model_->getActiveJointModels();
+  if (!group.empty())
+  {
+    const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
+    if (jmg)
+    {
+      active_joints = &jmg->getActiveJointModels();
+    }
+    else
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "There is no group with the name " << std::quoted(group) << "!");
+      return ros::Time(0.0);
+    }
+  }
+  auto oldest_state_time = ros::Time();
+  for (const moveit::core::JointModel* joint : *active_joints)
+  {
+    auto it = joint_time_.find(joint);
+    if (it == joint_time_.end())
+    {
+      ROS_DEBUG_NAMED(LOGNAME, "Joint '%s' has never been updated", joint->getName().c_str());
+    }
+    else
+    {
+      if (!oldest_state_time.isZero())
+      {
+        oldest_state_time = std::min(oldest_state_time, it->second);
+      }
+      else
+      {
+        oldest_state_time = it->second;
+      }
+    }
+  }
+  return oldest_state_time;
+}
+
+ros::Time CurrentStateMonitor::getCurrentStateTime(const std::string& group) const
 {
   boost::mutex::scoped_lock slock(state_update_lock_);
-  return current_state_time_;
+  return getCurrentStateTimeHelper(group);
 }
 
-std::pair<moveit::core::RobotStatePtr, ros::Time> CurrentStateMonitor::getCurrentStateAndTime() const
+std::pair<moveit::core::RobotStatePtr, ros::Time>
+CurrentStateMonitor::getCurrentStateAndTime(const std::string& group) const
 {
   boost::mutex::scoped_lock slock(state_update_lock_);
   moveit::core::RobotState* result = new moveit::core::RobotState(robot_state_);
-  return std::make_pair(moveit::core::RobotStatePtr(result), current_state_time_);
+  return std::make_pair(moveit::core::RobotStatePtr(result), getCurrentStateTimeHelper(group));
 }
 
 std::map<std::string, double> CurrentStateMonitor::getCurrentStateValues() const
@@ -185,11 +225,29 @@ std::string CurrentStateMonitor::getMonitoredTopic() const
 }
 
 bool CurrentStateMonitor::haveCompleteStateHelper(const ros::Time& oldest_allowed_update_time,
-                                                  std::vector<std::string>* missing_joints) const
+                                                  std::vector<std::string>* missing_joints,
+                                                  const std::string& group) const
 {
-  const std::vector<const moveit::core::JointModel*>& active_joints = robot_model_->getActiveJointModels();
+  const std::vector<const moveit::core::JointModel*>* active_joints = &robot_model_->getActiveJointModels();
+  if (!group.empty())
+  {
+    const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
+    if (jmg)
+    {
+      active_joints = &jmg->getActiveJointModels();
+    }
+    else
+    {
+      ROS_ERROR_STREAM_NAMED(LOGNAME, "There is no group with the name "
+                                          << std::quoted(group)
+                                          << ". All joints of the group are considered to be missing!");
+      if (missing_joints)
+        *missing_joints = robot_model_->getActiveJointModelNames();
+      return false;
+    }
+  }
   boost::mutex::scoped_lock slock(state_update_lock_);
-  for (const moveit::core::JointModel* joint : active_joints)
+  for (const moveit::core::JointModel* joint : *active_joints)
   {
     std::map<const moveit::core::JointModel*, ros::Time>::const_iterator it = joint_time_.find(joint);
     if (it == joint_time_.end())
@@ -203,7 +261,6 @@ bool CurrentStateMonitor::haveCompleteStateHelper(const ros::Time& oldest_allowe
     }
     else
       continue;
-
     if (missing_joints)
       missing_joints->push_back(joint->getName());
     else
@@ -217,9 +274,8 @@ bool CurrentStateMonitor::waitForCurrentState(const ros::Time t, double wait_tim
   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)
+  while (getCurrentStateTimeHelper() < t)
   {
     state_update_condition_.wait_for(lock, boost::chrono::nanoseconds((timeout - elapsed).toNSec()));
     elapsed = ros::WallTime::now() - start;
@@ -250,29 +306,21 @@ bool CurrentStateMonitor::waitForCompleteState(double wait_time) const
 
 bool CurrentStateMonitor::waitForCompleteState(const std::string& group, double wait_time) const
 {
-  if (waitForCompleteState(wait_time))
-    return true;
-  bool ok = true;
-
-  // check to see if we have a fully known state for the joints we want to record
+  double slept_time = 0.0;
+  double sleep_step_s = std::min(0.05, wait_time / 10.0);
+  ros::Duration sleep_step(sleep_step_s);
+  while (!haveCompleteState(group) && slept_time < wait_time)
+  {
+    sleep_step.sleep();
+    slept_time += sleep_step_s;
+  }
   std::vector<std::string> missing_joints;
-  if (!haveCompleteState(missing_joints))
+  if (!haveCompleteState(missing_joints, group))
   {
-    const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group);
-    if (jmg)
-    {
-      std::set<std::string> mj;
-      mj.insert(missing_joints.begin(), missing_joints.end());
-      const std::vector<std::string>& names = jmg->getJointModelNames();
-      bool ok = true;
-      for (std::size_t i = 0; ok && i < names.size(); ++i)
-        if (mj.find(names[i]) != mj.end())
-          ok = false;
-    }
-    else
-      ok = false;
+    ROS_ERROR_STREAM_NAMED(LOGNAME, std::quoted(group) << " has missing joints: " << boost::join(missing_joints, ","));
+    return false;
   }
-  return ok;
+  return true;
 }
 
 void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstPtr& joint_state)
@@ -291,7 +339,6 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstP
     boost::mutex::scoped_lock _(state_update_lock_);
     // read the received values, and update their time stamps
     std::size_t n = joint_state->name.size();
-    current_state_time_ = std::max(current_state_time_, joint_state->header.stamp);
     for (std::size_t i = 0; i < n; ++i)
     {
       const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_state->name[i]);
@@ -312,7 +359,6 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::JointStateConstP
                                            << "' is not newer than the previous state. Assuming your rosbag looped.");
         joint_time_.clear();
         joint_time_[jm] = joint_state->header.stamp;
-        current_state_time_ = joint_state->header.stamp;
       }
 
       if (robot_state_.getJointPositions(jm)[0] != joint_state->position[i])

moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_test.cpp

@@ -149,7 +149,14 @@ TEST_F(CurrentStateMonitorTest, IncrementalTimeStamps)
   EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime());
 
   sendJointStateAndWait(js_b);
-  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime()) << "older stamp made csm jump backwards in time";
+  EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())
+      << "older partial joint state was ignored in current state retrieval!";
+
+  js_b.position = { 0.25 };
+  js_b.header.stamp = ros::Time{ 10.5 };
+  sendJointStateAndWait(js_b);
+  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime())
+      << "older partial joint state was ignored in current state retrieval!";
 
   sendJointStateAndWait(js_ab);
   EXPECT_EQ(js_ab.header.stamp, csm->getCurrentStateTime()) << "newer stamp did not update csm";
@@ -163,6 +170,24 @@ TEST_F(CurrentStateMonitorTest, IncrementalTimeStamps)
   EXPECT_EQ(js_a_old.position[0], csm->getCurrentState()->getVariablePosition("a-b-joint"));
 }
 
+TEST_F(CurrentStateMonitorTest, NonMonotonicTimeStampsDueToPartialJoints)
+{
+  sendJointStateAndWait(js_a);
+  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime());
+
+  sendJointStateAndWait(js_b);
+  EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())
+      << "older partial joint state was ignored in current state retrieval!";
+  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime("group_a"))
+      << "Group is aware of the timestamp of non-group joints!";
+
+  js_b.position = { 0.25 };
+  js_b.header.stamp = ros::Time{ 13.0 };
+  sendJointStateAndWait(js_b);
+  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime())
+      << "older partial joint state was ignored in current state retrieval!";
+}
+
 int main(int argc, char** argv)
 {
   testing::InitGoogleTest(&argc, argv);