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

moveit · Nov 3, 2024 · e25da71 · e25da71
1 parent d5f763e
commit e25da71
Show file tree

Hide file tree

Showing 2 changed files with 31 additions and 4 deletions.
diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp
@@ -98,6 +98,9 @@ void CollisionMonitor::checkCollisions()
   double self_collision_scale, scene_collision_scale;
   const double log_val = -log(0.001);
 
+  // Get a read-only copy of the planning scene.
+  planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_);
+
   while (rclcpp::ok() && !stop_requested_)
   {
     const double self_velocity_scale_coefficient{ log_val / servo_params_.self_collision_proximity_threshold };
@@ -111,9 +114,6 @@ void CollisionMonitor::checkCollisions()
       // 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_,

diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp
@@ -62,14 +62,41 @@ 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 pose 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);
+    return planning_scene_monitor_->getPlanningScene()->getCurrentState().getGlobalLinkTransform(target_frame);
+  }
+
+  /// Helper function to get the joint configuration of a group.
+  Eigen::VectorXd getCurrentJointPositions(const std::string& group_name) const
+  {
+    std::vector<double> joint_positions;
+    const auto robot_state = planning_scene_monitor_->getPlanningScene()->getCurrentState();
+    robot_state.copyJointGroupPositions(group_name, joint_positions);
+    return Eigen::Map<Eigen::VectorXd>(joint_positions.data(), joint_positions.size());
   }
 
   std::shared_ptr<rclcpp::Node> servo_test_node_;