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

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

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e25da71
Wait for nonzero joint states in PSM in Servo CPP integration test
sea-bass Nov 3, 2024
8ef27b9
Fix fail wording
sea-bass Nov 3, 2024
f7c95c4
Revert locked planning scene change
sea-bass Nov 3, 2024
6aae20a
Lock down that planning scene
sea-bass Nov 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions moveit_ros/moveit_servo/src/collision_monitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_,
Expand Down
30 changes: 29 additions & 1 deletion moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.

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());
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!

}

std::shared_ptr<rclcpp::Node> servo_test_node_;
Expand Down
16 changes: 12 additions & 4 deletions moveit_ros/moveit_servo/tests/test_integration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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();
Expand All @@ -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();
Expand All @@ -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";
Expand All @@ -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);
Expand Down
Loading