mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
unittests
- provide simple hard-coded robot model - test ComputeIK::init()
This commit is contained in:
parent
f4adaa949a
commit
9e5e098339
@ -6,11 +6,11 @@
|
|||||||
if (CATKIN_ENABLE_TESTING)
|
if (CATKIN_ENABLE_TESTING)
|
||||||
find_package(rostest REQUIRED)
|
find_package(rostest REQUIRED)
|
||||||
|
|
||||||
add_library(gtest_utils gtest_value_printers.cpp)
|
add_library(gtest_utils gtest_value_printers.cpp models.cpp)
|
||||||
target_link_libraries(gtest_utils ${PROJECT_NAME})
|
target_link_libraries(gtest_utils ${PROJECT_NAME})
|
||||||
|
|
||||||
add_rostest_gtest(${PROJECT_NAME}-test-stage test_stage.launch test_stage.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-stage test_stage.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-stage ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-container test_container.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_utils gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} gtest_utils gtest_main)
|
||||||
|
|||||||
194
core/test/models.cpp
Normal file
194
core/test/models.cpp
Normal file
@ -0,0 +1,194 @@
|
|||||||
|
#include "models.h"
|
||||||
|
#include <moveit/robot_model/robot_model.h>
|
||||||
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
|
#include <urdf_parser/urdf_parser.h>
|
||||||
|
|
||||||
|
using namespace moveit::core;
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
const std::string R_MODEL0 =
|
||||||
|
"<?xml version=\"1.0\" ?>"
|
||||||
|
"<robot name=\"one_robot\">"
|
||||||
|
"<link name=\"base_link\">"
|
||||||
|
" <inertial>"
|
||||||
|
" <mass value=\"2.81\"/>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
|
||||||
|
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
|
||||||
|
" </inertial>"
|
||||||
|
" <collision name=\"my_collision\">"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
"<joint name=\"joint_a\" type=\"continuous\">"
|
||||||
|
" <axis xyz=\"0 0 1\"/>"
|
||||||
|
" <parent link=\"base_link\"/>"
|
||||||
|
" <child link=\"link_a\"/>"
|
||||||
|
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
|
||||||
|
"</joint>"
|
||||||
|
"<link name=\"link_a\">"
|
||||||
|
" <inertial>"
|
||||||
|
" <mass value=\"1.0\"/>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
|
||||||
|
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
|
||||||
|
" </inertial>"
|
||||||
|
" <collision>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
"<joint name=\"joint_b\" type=\"fixed\">"
|
||||||
|
" <parent link=\"link_a\"/>"
|
||||||
|
" <child link=\"link_b\"/>"
|
||||||
|
" <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
|
||||||
|
"</joint>"
|
||||||
|
"<link name=\"link_b\">"
|
||||||
|
" <inertial>"
|
||||||
|
" <mass value=\"1.0\"/>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
|
||||||
|
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
|
||||||
|
" </inertial>"
|
||||||
|
" <collision>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
" <joint name=\"joint_c\" type=\"prismatic\">"
|
||||||
|
" <axis xyz=\"1 0 0\"/>"
|
||||||
|
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
|
||||||
|
" <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
|
||||||
|
"soft_upper_limit=\"0.089\"/>"
|
||||||
|
" <parent link=\"link_b\"/>"
|
||||||
|
" <child link=\"link_c\"/>"
|
||||||
|
" <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
|
||||||
|
" </joint>"
|
||||||
|
"<link name=\"link_c\">"
|
||||||
|
" <inertial>"
|
||||||
|
" <mass value=\"1.0\"/>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
|
||||||
|
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
|
||||||
|
" </inertial>"
|
||||||
|
" <collision>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
" <joint name=\"mim_f\" type=\"prismatic\">"
|
||||||
|
" <axis xyz=\"1 0 0\"/>"
|
||||||
|
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
|
||||||
|
" <parent link=\"link_c\"/>"
|
||||||
|
" <child link=\"link_d\"/>"
|
||||||
|
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
|
||||||
|
" <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
|
||||||
|
" </joint>"
|
||||||
|
" <joint name=\"joint_f\" type=\"prismatic\">"
|
||||||
|
" <axis xyz=\"1 0 0\"/>"
|
||||||
|
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
|
||||||
|
" <parent link=\"link_d\"/>"
|
||||||
|
" <child link=\"link_e\"/>"
|
||||||
|
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
|
||||||
|
" </joint>"
|
||||||
|
"<link name=\"link_d\">"
|
||||||
|
" <collision>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
"<link name=\"link_e\">"
|
||||||
|
" <collision>"
|
||||||
|
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </collision>"
|
||||||
|
" <visual>"
|
||||||
|
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
|
||||||
|
" <geometry>"
|
||||||
|
" <box size=\"1 2 1\" />"
|
||||||
|
" </geometry>"
|
||||||
|
" </visual>"
|
||||||
|
"</link>"
|
||||||
|
"</robot>";
|
||||||
|
|
||||||
|
const std::string S_MODEL0 =
|
||||||
|
"<?xml version=\"1.0\" ?>"
|
||||||
|
"<robot name=\"one_robot\">"
|
||||||
|
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
|
||||||
|
"<group name=\"base_from_joints\">"
|
||||||
|
"<joint name=\"base_joint\"/>"
|
||||||
|
"<joint name=\"joint_a\"/>"
|
||||||
|
"<joint name=\"joint_c\"/>"
|
||||||
|
"</group>"
|
||||||
|
"<group name=\"mim_joints\">"
|
||||||
|
"<joint name=\"joint_f\"/>"
|
||||||
|
"<joint name=\"mim_f\"/>"
|
||||||
|
"</group>"
|
||||||
|
"<group name=\"base_with_subgroups\">"
|
||||||
|
"<group name=\"base_from_base_to_tip\"/>"
|
||||||
|
"<joint name=\"joint_c\"/>"
|
||||||
|
"</group>"
|
||||||
|
"<group name=\"base_from_base_to_tip\">"
|
||||||
|
"<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
|
||||||
|
"<joint name=\"base_joint\"/>"
|
||||||
|
"</group>"
|
||||||
|
"<group name=\"base_with_bad_subgroups\">"
|
||||||
|
"<group name=\"error\"/>"
|
||||||
|
"</group>"
|
||||||
|
"<end_effector name=\"eef\" parent_link=\"link_b\" group=\"mim_joints\" parent_group=\"base_from_base_to_tip\"/>"
|
||||||
|
"</robot>";
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
RobotModelPtr getModel()
|
||||||
|
{
|
||||||
|
urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(R_MODEL0);
|
||||||
|
srdf::ModelSharedPtr srdf_model(new srdf::Model());
|
||||||
|
srdf_model->initString(*urdf_model, S_MODEL0);
|
||||||
|
return RobotModelPtr(new RobotModel(urdf_model, srdf_model));
|
||||||
|
}
|
||||||
|
|
||||||
|
moveit::core::RobotModelPtr loadModel()
|
||||||
|
{
|
||||||
|
robot_model_loader::RobotModelLoader loader;
|
||||||
|
return loader.getModel();
|
||||||
|
}
|
||||||
13
core/test/models.h
Normal file
13
core/test/models.h
Normal file
@ -0,0 +1,13 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace core {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotModel)
|
||||||
|
} }
|
||||||
|
|
||||||
|
// get a hard-coded model
|
||||||
|
moveit::core::RobotModelPtr getModel();
|
||||||
|
|
||||||
|
// load a model from robot_description
|
||||||
|
moveit::core::RobotModelPtr loadModel();
|
||||||
@ -1,10 +1,12 @@
|
|||||||
#include <moveit/task_constructor/stage_p.h>
|
#include "models.h"
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
|
||||||
#include <ros/init.h>
|
|
||||||
|
|
||||||
|
#include <moveit/task_constructor/stage_p.h>
|
||||||
|
#include <moveit/task_constructor/stages/compute_ik.h>
|
||||||
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
#include <geometry_msgs/PoseStamped.h>
|
||||||
|
|
||||||
|
#include <ros/console.h>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <initializer_list>
|
|
||||||
|
|
||||||
using namespace moveit::task_constructor;
|
using namespace moveit::task_constructor;
|
||||||
|
|
||||||
@ -15,27 +17,31 @@ class GeneratorMockup : public Generator {
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
GeneratorMockup() : Generator("generator") {
|
GeneratorMockup() : Generator("generator") {
|
||||||
robot_model_loader::RobotModelLoader loader;
|
|
||||||
ps.reset(new planning_scene::PlanningScene(loader.getModel()));
|
|
||||||
|
|
||||||
prev.reset(new Interface);
|
prev.reset(new Interface);
|
||||||
next.reset(new Interface);
|
next.reset(new Interface);
|
||||||
pimpl()->setPrevEnds(prev);
|
pimpl()->setPrevEnds(prev);
|
||||||
pimpl()->setNextStarts(next);
|
pimpl()->setNextStarts(next);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void init(const moveit::core::RobotModelConstPtr &robot_model) {
|
||||||
|
ps.reset((new planning_scene::PlanningScene(robot_model)));
|
||||||
|
}
|
||||||
|
|
||||||
bool canCompute() const override { return true; }
|
bool canCompute() const override { return true; }
|
||||||
bool compute() override {
|
bool compute() override {
|
||||||
spawn(InterfaceState(ps), 0.0);
|
InterfaceState state(ps);
|
||||||
|
state.properties().set("target_pose", geometry_msgs::PoseStamped());
|
||||||
|
spawn(std::move(state), 0.0);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
TEST(Stage, registerCallbacks) {
|
TEST(Stage, registerCallbacks) {
|
||||||
int argc = 0;
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
||||||
char *argv = nullptr;
|
|
||||||
ros::init(argc, &argv, "testLocalTaskModel");
|
|
||||||
|
|
||||||
GeneratorMockup g;
|
GeneratorMockup g;
|
||||||
|
g.init(getModel());
|
||||||
|
|
||||||
uint called = 0;
|
uint called = 0;
|
||||||
auto cb = [&called](const SolutionBase &s) {
|
auto cb = [&called](const SolutionBase &s) {
|
||||||
++called;
|
++called;
|
||||||
@ -50,3 +56,33 @@ TEST(Stage, registerCallbacks) {
|
|||||||
g.compute();
|
g.compute();
|
||||||
EXPECT_EQ(called, 1);
|
EXPECT_EQ(called, 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
TEST(ComputeIK, init) {
|
||||||
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
||||||
|
|
||||||
|
auto g = std::make_unique<GeneratorMockup>();
|
||||||
|
stages::ComputeIK ik("ik", std::move(g));
|
||||||
|
moveit::core::RobotModelPtr robot_model = getModel();
|
||||||
|
auto& props = ik.properties();
|
||||||
|
|
||||||
|
// neither eef nor group defined should not throw
|
||||||
|
EXPECT_NO_THROW(ik.init(robot_model));
|
||||||
|
|
||||||
|
// invalid eef should throw
|
||||||
|
props.set("eef", std::string("undefined"));
|
||||||
|
EXPECT_THROW(ik.init(robot_model), InitStageException);
|
||||||
|
|
||||||
|
// valid eef should not throw
|
||||||
|
props.set("eef", std::string("eef"));
|
||||||
|
EXPECT_NO_THROW(ik.init(robot_model));
|
||||||
|
|
||||||
|
props.set("eef", boost::any());
|
||||||
|
|
||||||
|
// invalid group should throw
|
||||||
|
props.set("group", std::string("undefined"));
|
||||||
|
EXPECT_THROW(ik.init(robot_model), InitStageException);
|
||||||
|
|
||||||
|
// valid group should not throw
|
||||||
|
props.set("group", std::string("base_from_base_to_tip"));
|
||||||
|
EXPECT_NO_THROW(ik.init(robot_model));
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user