Wait for nonzero joint states in PSM in Servo CPP integration test (m…

…oveit#3056)

* Wait for nonzero joint states in PSM in Servo CPP integration test

* Fix fail wording

* Revert locked planning scene change

* Lock down that planning scene

moveit_ros/moveit_servo/src/collision_monitor.cpp

@@ -105,15 +105,15 @@ void CollisionMonitor::checkCollisions()
 
     if (servo_params_.check_collisions)
     {
+      // Get a read-only copy of the planning scene.
+      planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+
       // Fetch latest robot state using planning scene instead of state monitor due to
       // https://github.com/moveit/moveit2/issues/2748
-      robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState();
+      robot_state_ = locked_scene->getCurrentState();
       // This must be called before doing collision checking.
       robot_state_.updateCollisionBodyTransforms();
 
-      // Get a read-only copy of planning scene.
-      planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
-
       // Check collision with environment.
       scene_collision_result_.clear();
       locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_,

moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp

@@ -62,14 +62,42 @@ class ServoCppFixture : public testing::Test
 
     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)
+    {
+      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++;
+    }
+
     servo_test_instance_ =
         std::make_shared<moveit_servo::Servo>(servo_test_node_, servo_param_listener_, planning_scene_monitor_);
   }
 
   /// Helper function to get the current pose of a specified frame.
   Eigen::Isometry3d getCurrentPose(const std::string& target_frame) const
   {
-    return planning_scene_monitor_->getStateMonitor()->getCurrentState()->getGlobalLinkTransform(target_frame);
+    planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+    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_;

moveit_ros/moveit_servo/tests/test_integration.cpp

@@ -46,10 +46,12 @@ namespace
 
 TEST_F(ServoCppFixture, JointJogTest)
 {
+  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
+
   moveit_servo::StatusCode status_curr, status_next, status_initial;
   moveit_servo::JointJogCommand joint_jog_z{ { "panda_joint7" }, { 1.0 } };
   moveit_servo::JointJogCommand zero_joint_jog;
-  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 
   // Compute next state.
   servo_test_instance_->setCommandType(moveit_servo::CommandType::JOINT_JOG);
@@ -72,10 +74,12 @@ TEST_F(ServoCppFixture, JointJogTest)
 
 TEST_F(ServoCppFixture, TwistTest)
 {
+  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
+
   moveit_servo::StatusCode status_curr, status_next, status_initial;
   moveit_servo::TwistCommand twist_non_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
   moveit_servo::TwistCommand twist_zero{ "panda_link0", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
-  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 
   servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
   status_initial = servo_test_instance_->getStatus();
@@ -97,10 +101,12 @@ TEST_F(ServoCppFixture, TwistTest)
 
 TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
 {
+  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
+
   moveit_servo::StatusCode status_curr, status_next, status_initial;
   moveit_servo::TwistCommand twist_non_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.1 } };
   moveit_servo::TwistCommand twist_zero{ "panda_link8", { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } };
-  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 
   servo_test_instance_->setCommandType(moveit_servo::CommandType::TWIST);
   status_initial = servo_test_instance_->getStatus();
@@ -122,6 +128,9 @@ TEST_F(ServoCppFixture, NonPlanningFrameTwistTest)
 
 TEST_F(ServoCppFixture, PoseTest)
 {
+  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+  auto robot_state = std::make_shared<moveit::core::RobotState>(locked_scene->getCurrentState());
+
   moveit_servo::StatusCode status_curr, status_next, status_initial;
   moveit_servo::PoseCommand zero_pose, non_zero_pose;
   zero_pose.frame_id = "panda_link0";
@@ -135,7 +144,6 @@ TEST_F(ServoCppFixture, PoseTest)
   status_initial = servo_test_instance_->getStatus();
   ASSERT_EQ(status_initial, moveit_servo::StatusCode::NO_WARNING);
 
-  moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
   moveit_servo::KinematicState curr_state = servo_test_instance_->getNextJointState(robot_state, zero_pose);
   status_curr = servo_test_instance_->getStatus();
   ASSERT_EQ(status_curr, moveit_servo::StatusCode::NO_WARNING);