Wait for nonzero joint states in PSM in Servo CPP integration test#3056
Wait for nonzero joint states in PSM in Servo CPP integration test#3056
Conversation
|
@ibrahiminfinite tagging you too! |
|
The results seem encouraging I ran CI 15 times, seeing 2 failures not related to this test. Once, one of the jobs simply got stuck so I cancelled it. The other time, some other kinematics set failed -- unsure why, maybe another super rare flake. |
mikeferguson
left a comment
There was a problem hiding this comment.
The only thing I will note is that the API docs for getPlanningScene() says "Avoid this function! Returns an unsafe pointer to the current planning scene."
Looking at the API docs a bit more - should we actually maybe be using a locked planning scene? |
|
Yeah, probably should change it -- I'll poke through this when I get a chance. Thanks! |
| 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()); |
There was a problem hiding this comment.
This returns a pointer to the local vector joint_positions, which is destroyed when leaving the function!
|
|
||
| planning_scene_monitor_ = moveit_servo::createPlanningSceneMonitor(servo_test_node_, servo_params_); | ||
|
|
||
| // Wait until the joint configuration is nonzero before starting MoveIt Servo. |
There was a problem hiding this comment.
There is a function for this:
planning_scene_monitor_->getStateMonitor()->waitForCompleteState(group_name, wait_time);
There was a problem hiding this comment.
But we're not using the state monitor anymore, which was the original issue right?
There was a problem hiding this comment.
The issue was not with the StateMonitor itself, but asking for the current state before it was updated the first time.
…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
* Cleanup moveit#3056 * Switch to PSM's waitForCurrentRobotState() * Back to StateMonitor usage and explicitly propagate state update to PlanningScene
Description
I finally chased down the source of the flakiness of the servo integration tests in #3005... I think.
Turns out that the collision monitor thread gets the robot state with the following due to #2747 / #2748:
robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState();However, the Servo integration CPP tests were using
Switching to the first implementation in the tests caused our "flake" to be a consistently reproducible failure. The initial robot state was all zero since the test instantiates servo and immediately computes some stuff really quick. Actually, what we thought was a flake in failing was the collision monitor sometimes getting a chance to run once before servo computed, and immediately tell us the robot was at in collision, and fail!
So this PR adds some waits for the robot joint configuration to become nonzero.
This was not a problem in the ROS integration tests since there is a function that waits for a robotstate message.
Closes #3005
Checklist