diff --git a/core/test/pick_pa10.cpp b/core/test/pick_pa10.cpp index 00eb6a07..bc848c1e 100644 --- a/core/test/pick_pa10.cpp +++ b/core/test/pick_pa10.cpp @@ -53,6 +53,7 @@ TEST(PA10, pick) { { auto scene = std::make_shared(t.getRobotModel()); auto& state = scene->getCurrentStateNonConst(); + state.setToDefaultValues(); // initialize state state.setToDefaultValues(state.getJointModelGroup("left_arm"), "home"); state.setToDefaultValues(state.getJointModelGroup("right_arm"), "home"); state.update(); @@ -179,8 +180,5 @@ int main(int argc, char** argv){ ros::AsyncSpinner spinner(1); spinner.start(); - // wait some time for move_group to come up - ros::WallDuration(5.0).sleep(); - return RUN_ALL_TESTS(); }