mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Simplify test code
This commit is contained in:
parent
f5987dd616
commit
464c45f630
@ -36,29 +36,28 @@ struct PandaMoveTo : public testing::Test
|
|||||||
TEST_F(PandaMoveTo, successExecution) {
|
TEST_F(PandaMoveTo, successExecution) {
|
||||||
move_to->setGoal("extended");
|
move_to->setGoal("extended");
|
||||||
EXPECT_TRUE(t.plan());
|
EXPECT_TRUE(t.plan());
|
||||||
auto execute_result = t.execute(*t.solutions().front());
|
auto result = t.execute(*t.solutions().front());
|
||||||
EXPECT_TRUE(execute_result.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
|
EXPECT_TRUE(result.val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS);
|
||||||
}
|
}
|
||||||
|
|
||||||
// After the arm successfully moves to "extended" try to move back to "ready" and make sure preempt() woks as expected
|
// After the arm successfully moved to "extended", move back to "ready" and make sure preempt() works as expected
|
||||||
TEST_F(PandaMoveTo, preemptExecution) {
|
TEST_F(PandaMoveTo, preemptExecution) {
|
||||||
move_to->setGoal("ready");
|
move_to->setGoal("ready");
|
||||||
EXPECT_TRUE(t.plan());
|
EXPECT_TRUE(t.plan());
|
||||||
// extract the expected execution time (for this task its in the last sub_trajectory)
|
// extract the expected execution time (for this task its in the last sub_trajectory)
|
||||||
moveit_task_constructor_msgs::msg::Solution s;
|
moveit_task_constructor_msgs::msg::Solution s;
|
||||||
t.solutions().front()->toMsg(s, nullptr);
|
t.solutions().front()->toMsg(s, nullptr);
|
||||||
auto exec_duration = s.sub_trajectory.back().trajectory.joint_trajectory.points.back().time_from_start;
|
rclcpp::Duration exec_duration{ s.sub_trajectory.back().trajectory.joint_trajectory.points.back().time_from_start };
|
||||||
|
|
||||||
|
moveit::core::MoveItErrorCode result;
|
||||||
|
std::thread execute_thread{ [this, &result]() { result = t.execute(*t.solutions().front()); } };
|
||||||
|
|
||||||
moveit::core::MoveItErrorCode execute_result;
|
|
||||||
execute_result.val = execute_result.UNDEFINED;
|
|
||||||
std::thread execute_thread{ [this, &execute_result]() { execute_result = t.execute(*t.solutions().front()); } };
|
|
||||||
// cancel the trajectory half way through the expected execution time
|
// cancel the trajectory half way through the expected execution time
|
||||||
rclcpp::sleep_for(std::chrono::duration_cast<std::chrono::nanoseconds>(
|
rclcpp::sleep_for(exec_duration.to_chrono<std::chrono::milliseconds>() / 2);
|
||||||
(std::chrono::seconds(exec_duration.sec) + std::chrono::nanoseconds(exec_duration.nanosec)) / 2));
|
|
||||||
t.preempt();
|
t.preempt();
|
||||||
execute_thread.join();
|
execute_thread.join();
|
||||||
|
|
||||||
EXPECT_TRUE(execute_result.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED);
|
EXPECT_TRUE(result.val == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED);
|
||||||
}
|
}
|
||||||
|
|
||||||
int main(int argc, char** argv) {
|
int main(int argc, char** argv) {
|
||||||
|
Loading…
Reference in New Issue
Block a user