Cleanup #3056 (#3058)

* Cleanup #3056

* Switch to PSM's waitForCurrentRobotState()

* Back to StateMonitor usage

and explicitly propagate state update to PlanningScene

moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp

@@ -61,24 +61,13 @@ class ServoCppFixture : public testing::Test
     servo_params_ = servo_param_listener_->get_params();
 
     planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_);
-
-    // Wait until the joint configuration is nonzero before starting MoveIt Servo.
-    int num_tries = 0;
-    const int max_tries = 20;
-    while (true)
+    // Wait for complete state update before starting MoveIt Servo.
+    if (!planning_scene_monitor_->getStateMonitor()->waitForCompleteState("panda_arm", 1.0))
     {
-      const auto q = getCurrentJointPositions("panda_arm");
-      if (q.norm() > 0.0)
-      {
-        break;
-      }
-      if (num_tries > max_tries)
-      {
-        FAIL() << "Robot joint configuration did not reach expected state after some time. Test is flaky.";
-      }
-      std::this_thread::sleep_for(std::chrono::milliseconds(100));
-      num_tries++;
+      FAIL() << "Could not retrieve complete robot state";
     }
+    // Forward state update to planning scene
+    planning_scene_monitor_->updateSceneWithCurrentState();
 
     servo_test_instance_ =
         std::make_shared<moveit_servo::Servo>(servo_test_node_, servo_param_listener_, planning_scene_monitor_);
@@ -91,15 +80,6 @@ class ServoCppFixture : public testing::Test
     return locked_scene->getCurrentState().getGlobalLinkTransform(target_frame);
   }
 
-  /// Helper function to get the joint configuration of a group.
-  Eigen::VectorXd getCurrentJointPositions(const std::string& group_name) const
-  {
-    planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
-    std::vector<double> joint_positions;
-    locked_scene->getCurrentState().copyJointGroupPositions(group_name, joint_positions);
-    return Eigen::Map<Eigen::VectorXd>(joint_positions.data(), joint_positions.size());
-  }
-
   std::shared_ptr<rclcpp::Node> servo_test_node_;
   std::shared_ptr<const servo::ParamListener> servo_param_listener_;
   servo::Params servo_params_;