add some tests for MoveTo

This commit is contained in:
v4hn 2021-10-05 09:51:45 +02:00
parent dfe746b9bb
commit ef86799f27
3 changed files with 116 additions and 0 deletions

View File

@ -33,6 +33,9 @@ if (CATKIN_ENABLE_TESTING)
mtc_add_gmock(test_cost_queue.cpp)
mtc_add_gmock(test_interface_state.cpp)
add_rostest_gtest(${PROJECT_NAME}-test-move-to move_to.test test_move_to.cpp)
target_link_libraries(${PROJECT_NAME}-test-move-to ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
# building these integration tests works without moveit config packages
add_executable(pick_ur5 pick_ur5.cpp)
target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest)

7
core/test/move_to.test Normal file
View File

@ -0,0 +1,7 @@
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core" type="moveit_task_constructor_core-test-move-to" test-name="move_to"/>
</launch>

106
core/test/test_move_to.cpp Normal file
View File

@ -0,0 +1,106 @@
#include "models.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit_msgs/RobotState.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/console.h>
#include <gtest/gtest.h>
using namespace moveit::task_constructor;
using namespace planning_scene;
using namespace moveit::core;
constexpr double TAU{ 2 * M_PI };
// provide a basic test fixture that prepares a Task
struct PandaMoveTo : public testing::Test
{
Task t;
stages::MoveTo* move_to;
PlanningScenePtr scene;
PandaMoveTo() {
t.setRobotModel(loadModel());
scene = std::make_shared<PlanningScene>(t.getRobotModel());
scene->getCurrentStateNonConst().setToDefaultValues();
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
t.add(std::make_unique<stages::FixedState>("start", scene));
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
move_to = move.get();
move_to->setGroup("panda_arm");
t.add(std::move(move));
}
};
#define EXPECT_ONE_SOLUTION \
{ \
EXPECT_TRUE(t.plan()); \
EXPECT_EQ(t.solutions().size(), 1u); \
}
TEST_F(PandaMoveTo, namedTarget) {
move_to->setGoal("extended");
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, mapTarget) {
move_to->setGoal(std::map<std::string, double>{ { "panda_joint1", TAU / 8 }, { "panda_joint2", TAU / 8 } });
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, stateTarget) {
move_to->setGoal([]() {
moveit_msgs::RobotState state;
state.is_diff = true;
state.joint_state.name = { "panda_joint1", "panda_joint2" };
state.joint_state.position = { TAU / 8, TAU / 8 };
return state;
}());
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, DISABLED_pointTarget) {
move_to->setGoal([this]() {
RobotState state{ scene->getCurrentState() };
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
auto frame{ state.getFrameTransform("panda_link8") };
geometry_msgs::PointStamped point;
point.header.frame_id = scene->getPlanningFrame();
point.point = tf2::toMsg(Eigen::Vector3d{ frame.translation() });
return point;
}());
EXPECT_ONE_SOLUTION;
}
TEST_F(PandaMoveTo, DISABLED_poseTarget) {
move_to->setGoal([this]() {
RobotState state{ scene->getCurrentState() };
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
auto frame{ state.getFrameTransform("panda_link8") };
geometry_msgs::PoseStamped pose;
pose.header.frame_id = scene->getPlanningFrame();
pose.pose = tf2::toMsg(frame);
return pose;
}());
EXPECT_ONE_SOLUTION;
}
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);
ros::init(argc, argv, "move_to_test");
ros::AsyncSpinner spinner(1);
spinner.start();
return RUN_ALL_TESTS();
}