Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

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

Merged
merged 4 commits into from
Nov 4, 2024
Merged

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

merged 4 commits into from
Nov 4, 2024

Conversation

sea-bass
Copy link
Contributor

@sea-bass sea-bass commented Nov 3, 2024 •
edited
Loading

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

/// 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);
}

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

  • Required by CI: Code is auto formatted using clang-format
  • Extend the tutorials / documentation reference
  • Document API changes relevant to the user in the MIGRATION.md notes
  • Create tests, which fail without this PR reference
  • Include a screenshot if changing a GUI
  • While waiting for someone to review your request, please help review another open pull request to support the maintainers

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 3, 2024

@ibrahiminfinite tagging you too!

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 3, 2024 •
edited
Loading

The results seem encouraging
https://github.com/moveit/moveit2/actions/runs/11648343245

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
mikeferguson approved these changes Nov 3, 2024
View reviewed changes
Copy link
Contributor

@mikeferguson mikeferguson left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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."

@mikeferguson
Copy link
Contributor

image

Looking at the API docs a bit more - should we actually maybe be using a locked planning scene?

@sea-bass
Copy link
Contributor Author

sea-bass commented Nov 3, 2024

Yeah, probably should change it -- I'll poke through this when I get a chance. Thanks!

@sea-bass sea-bass added this pull request to the merge queue Nov 3, 2024
@sea-bass sea-bass removed this pull request from the merge queue due to a manual request Nov 3, 2024
@sjahr sjahr added this pull request to the merge queue Nov 4, 2024
Merged via the queue into main with commit b3b1f73 Nov 4, 2024
10 of 14 checks passed
@sjahr sjahr deleted the fix-servo-flake branch November 4, 2024 10:13
rhaschke added a commit to rhaschke/moveit2 that referenced this pull request Nov 4, 2024
@rhaschke
Cleanup moveit#3056
ddfa7ff
@rhaschke rhaschke mentioned this pull request Nov 4, 2024
Merged
rhaschke
rhaschke reviewed Nov 4, 2024
View reviewed changes
Copy link
Contributor

@rhaschke rhaschke left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Great that you could track down that issue. Unfortunately, you introduced a new issue. Addressed in #3058.

moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp
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());
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This returns a pointer to the local vector joint_positions, which is destroyed when leaving the function!

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.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

There is a function for this:
planning_scene_monitor_->getStateMonitor()->waitForCompleteState(group_name, wait_time);

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But we're not using the state monitor anymore, which was the original issue right?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The issue was not with the StateMonitor itself, but asking for the current state before it was updated the first time.

github-merge-queue bot pushed a commit that referenced this pull request Nov 5, 2024
@rhaschke
Cleanup #3056 (#3058)
36487d1 
* Cleanup #3056

* Switch to PSM's waitForCurrentRobotState()

* Back to StateMonitor usage

and explicitly propagate state update to PlanningScene
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@rhaschke rhaschke rhaschke left review comments

@sjahr sjahr sjahr approved these changes

@mikeferguson mikeferguson mikeferguson approved these changes

@henningkayser henningkayser Awaiting requested review from henningkayser

@AndyZe AndyZe Awaiting requested review from AndyZe

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

ServoCppFixture.PoseTest is flaky
4 participants
@sea-bass @mikeferguson @rhaschke @sjahr