diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index c3a886cff8..f1e8c8c911 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/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_, diff --git a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp index 7e98f8e680..86e9c657ff 100644 --- a/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp +++ b/moveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp @@ -62,6 +62,24 @@ 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(servo_test_node_, servo_param_listener_, planning_scene_monitor_); } @@ -69,7 +87,17 @@ class ServoCppFixture : public testing::Test /// 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 joint_positions; + locked_scene->getCurrentState().copyJointGroupPositions(group_name, joint_positions); + return Eigen::Map(joint_positions.data(), joint_positions.size()); } std::shared_ptr servo_test_node_; diff --git a/moveit_ros/moveit_servo/tests/test_integration.cpp b/moveit_ros/moveit_servo/tests/test_integration.cpp index d04587a9ab..645b2835cf 100644 --- a/moveit_ros/moveit_servo/tests/test_integration.cpp +++ b/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(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(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(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(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);