Fix logic errors in getCurrentStateTimeHelper (moveit#3672)

- Return the oldest time a joint from the group was updated - If joints never received an update, the return value should be zero - Joint updates received via tf_static should be always considered recent Co-authored-by: Robert Haschke <[email protected]>
ubi-agni · Jan 6, 2025 · 54e9df7 · 54e9df7
1 parent b6bde10
commit 54e9df7
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 18 deletions.
diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
@@ -89,28 +89,22 @@ ros::Time CurrentStateMonitor::getCurrentStateTimeHelper(const std::string& grou
     else
     {
       ROS_ERROR_STREAM_NAMED(LOGNAME, "There is no group with the name " << std::quoted(group) << "!");
-      return ros::Time(0.0);
+      return ros::Time();
     }
   }
-  auto oldest_state_time = ros::Time();
+  auto oldest_state_time = ros::Time::now();
   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;
-      }
+      ROS_DEBUG_NAMED(LOGNAME, "Joint '%s' has never been updated (and possibly others as well)",
+                      joint->getName().c_str());
+      return ros::Time();  // return zero if any joint was never updated
     }
+    // update oldest_state_time for all joints except those updated via tf_static
+    else if (!it->second.isZero())
+      oldest_state_time = std::min(oldest_state_time, it->second);
   }
   return oldest_state_time;
 }

diff --git a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_test.cpp
@@ -146,7 +146,7 @@ TEST_F(CurrentStateMonitorTest, StateUpdateTest)
 TEST_F(CurrentStateMonitorTest, IncrementalTimeStamps)
 {
   sendJointStateAndWait(js_a);
-  EXPECT_EQ(js_a.header.stamp, csm->getCurrentStateTime());
+  EXPECT_EQ(csm->getCurrentStateTime(), ros::Time()) << "incomplete state should have 0 time";
 
   sendJointStateAndWait(js_b);
   EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())
@@ -165,15 +165,14 @@ TEST_F(CurrentStateMonitorTest, IncrementalTimeStamps)
   sendJointStateAndWait(js_a_old);
   EXPECT_FALSE(csm->haveCompleteState())
       << "jumped back in time for a known joint, but csm still claims it knows all joints";
-  EXPECT_EQ(js_a_old.header.stamp, csm->getCurrentStateTime())
-      << "jumping back for a known joint did not reset state time";
+  EXPECT_EQ(csm->getCurrentStateTime(), ros::Time()) << "jumping back for a known joint did not reset state time";
   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());
+  EXPECT_EQ(csm->getCurrentStateTime(), ros::Time()) << "incomplete state should have 0 time";
 
   sendJointStateAndWait(js_b);
   EXPECT_EQ(js_b.header.stamp, csm->getCurrentStateTime())