From 54e9df71a0f7d89ed0372fa629e6d3843503fda9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Michael=20G=C3=B6rner?= Date: Mon, 6 Jan 2025 22:51:46 +0100 Subject: [PATCH] Fix logic errors in getCurrentStateTimeHelper (#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 --- .../src/current_state_monitor.cpp | 22 +++++++------------ .../test/current_state_monitor_test.cpp | 7 +++--- 2 files changed, 11 insertions(+), 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 index 8f515b50c4..f1c6988acc 100644 --- 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 index df5f0e0d81..b2088d825f 100644 --- 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())