mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Use RobotModelBuilder to simplify tests (#225)
* Simplify RobotModel definition using RobotModelBuilder * Silent RobotModel errors once in models.cpp
This commit is contained in:
parent
8a913d8bf3
commit
f99d03386a
@ -16,7 +16,7 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
|
||||||
@ -25,7 +25,7 @@ if (CATKIN_ENABLE_TESTING)
|
|||||||
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_utils gtest_main)
|
||||||
|
|
||||||
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)
|
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)
|
||||||
target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
|
||||||
|
|||||||
@ -1,187 +1,20 @@
|
|||||||
#include "models.h"
|
#include "models.h"
|
||||||
#include <moveit/robot_model/robot_model.h>
|
#include <moveit/utils/robot_model_test_utils.h>
|
||||||
#include <moveit/robot_model_loader/robot_model_loader.h>
|
#include <moveit/robot_model_loader/robot_model_loader.h>
|
||||||
#include <urdf_parser/urdf_parser.h>
|
|
||||||
|
|
||||||
using namespace moveit::core;
|
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>";
|
|
||||||
} // namespace
|
|
||||||
|
|
||||||
RobotModelPtr getModel() {
|
RobotModelPtr getModel() {
|
||||||
urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(R_MODEL0);
|
// suppress RobotModel errors and warnings
|
||||||
srdf::ModelSharedPtr srdf_model(new srdf::Model());
|
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal);
|
||||||
srdf_model->initString(*urdf_model, S_MODEL0);
|
|
||||||
return RobotModelPtr(new RobotModel(urdf_model, srdf_model));
|
// create dummy robot model
|
||||||
|
moveit::core::RobotModelBuilder builder("robot", "base");
|
||||||
|
builder.addChain("base->link1->link2->tip", "continuous");
|
||||||
|
builder.addGroupChain("base", "link2", "group");
|
||||||
|
builder.addGroupChain("link2", "tip", "eef_group");
|
||||||
|
builder.addEndEffector("eef", "link2", "group", "eef_group");
|
||||||
|
return builder.build();
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit::core::RobotModelPtr loadModel() {
|
moveit::core::RobotModelPtr loadModel() {
|
||||||
|
|||||||
@ -3,8 +3,8 @@
|
|||||||
#include <moveit/task_constructor/task_p.h>
|
#include <moveit/task_constructor/task_p.h>
|
||||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/utils/robot_model_test_utils.h>
|
|
||||||
|
|
||||||
|
#include "models.h"
|
||||||
#include "gtest_value_printers.h"
|
#include "gtest_value_printers.h"
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
#include <initializer_list>
|
#include <initializer_list>
|
||||||
@ -686,11 +686,7 @@ TEST(Task, move) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(Task, reuse) {
|
TEST(Task, reuse) {
|
||||||
// create dummy robot model
|
moveit::core::RobotModelConstPtr robot_model = getModel();
|
||||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
|
||||||
builder.addChain("base->a->b->c", "continuous");
|
|
||||||
builder.addGroupChain("base", "c", "group");
|
|
||||||
moveit::core::RobotModelConstPtr robot_model = builder.build();
|
|
||||||
|
|
||||||
Task t("first");
|
Task t("first");
|
||||||
t.setRobotModel(robot_model);
|
t.setRobotModel(robot_model);
|
||||||
@ -725,13 +721,8 @@ TEST(Task, reuse) {
|
|||||||
|
|
||||||
TEST(Task, timeout) {
|
TEST(Task, timeout) {
|
||||||
MOCK_ID = 0;
|
MOCK_ID = 0;
|
||||||
// create dummy robot model
|
|
||||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
|
||||||
builder.addChain("base->a->b->c", "continuous");
|
|
||||||
builder.addGroupChain("base", "c", "group");
|
|
||||||
|
|
||||||
Task t;
|
Task t;
|
||||||
t.setRobotModel(builder.build());
|
t.setRobotModel(getModel());
|
||||||
|
|
||||||
auto timeout = std::chrono::milliseconds(10);
|
auto timeout = std::chrono::milliseconds(10);
|
||||||
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned
|
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned
|
||||||
|
|||||||
@ -172,7 +172,6 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
TEST(CostTerm, SetLambdaCostTerm) {
|
TEST(CostTerm, SetLambdaCostTerm) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
Standalone<SerialContainer> container(robot);
|
||||||
@ -198,7 +197,6 @@ TEST(CostTerm, SetLambdaCostTerm) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, CostOverwrite) {
|
TEST(CostTerm, CostOverwrite) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
const moveit::core::RobotModelConstPtr robot{ getModel() };
|
||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
Standalone<SerialContainer> container(robot);
|
||||||
@ -215,7 +213,6 @@ TEST(CostTerm, CostOverwrite) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, StageTypes) {
|
TEST(CostTerm, StageTypes) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
|
|
||||||
Standalone<SerialContainer> container(robot);
|
Standalone<SerialContainer> container(robot);
|
||||||
@ -237,7 +234,6 @@ TEST(CostTerm, StageTypes) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, PassThroughUsesCost) {
|
TEST(CostTerm, PassThroughUsesCost) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
@ -253,7 +249,6 @@ TEST(CostTerm, PassThroughUsesCost) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, PassThroughOverwritesCost) {
|
TEST(CostTerm, PassThroughOverwritesCost) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
@ -271,7 +266,6 @@ TEST(CostTerm, PassThroughOverwritesCost) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, PassThroughCanModifyCost) {
|
TEST(CostTerm, PassThroughCanModifyCost) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
moveit::core::RobotModelPtr robot{ getModel() };
|
moveit::core::RobotModelPtr robot{ getModel() };
|
||||||
Standalone<stages::PassThrough> container(robot);
|
Standalone<stages::PassThrough> container(robot);
|
||||||
|
|
||||||
@ -287,7 +281,6 @@ TEST(CostTerm, PassThroughCanModifyCost) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, CompositeSolutions) {
|
TEST(CostTerm, CompositeSolutions) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
Standalone<SerialContainer> container{ getModel() };
|
Standalone<SerialContainer> container{ getModel() };
|
||||||
|
|
||||||
{
|
{
|
||||||
@ -330,7 +323,6 @@ TEST(CostTerm, CompositeSolutions) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(CostTerm, CompositeSolutionsContainerCost) {
|
TEST(CostTerm, CompositeSolutionsContainerCost) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
Standalone<SerialContainer> container{ getModel() };
|
Standalone<SerialContainer> container{ getModel() };
|
||||||
|
|
||||||
auto s1{ std::make_unique<ForwardMockup>() };
|
auto s1{ std::make_unique<ForwardMockup>() };
|
||||||
|
|||||||
@ -1,7 +1,8 @@
|
|||||||
#include <moveit/task_constructor/storage.h>
|
#include <moveit/task_constructor/storage.h>
|
||||||
#include <moveit/utils/robot_model_test_utils.h>
|
#include <moveit/task_constructor/stage_p.h>
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
|
#include "models.h"
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <iterator>
|
#include <iterator>
|
||||||
@ -28,13 +29,6 @@ TEST(InterfaceStatePriority, compare) {
|
|||||||
EXPECT_TRUE(Prio(0, inf) > Prio(0, 0));
|
EXPECT_TRUE(Prio(0, inf) > Prio(0, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
moveit::core::RobotModelConstPtr getModel() {
|
|
||||||
ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error);
|
|
||||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
|
||||||
builder.addChain("base->tip", "continuous");
|
|
||||||
return builder.build();
|
|
||||||
}
|
|
||||||
|
|
||||||
using Prio = InterfaceState::Priority;
|
using Prio = InterfaceState::Priority;
|
||||||
|
|
||||||
// Interface that also stores passed states
|
// Interface that also stores passed states
|
||||||
|
|||||||
@ -5,6 +5,7 @@
|
|||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
#include <moveit/utils/robot_model_test_utils.h>
|
#include <moveit/utils/robot_model_test_utils.h>
|
||||||
|
|
||||||
|
#include "models.h"
|
||||||
#include <list>
|
#include <list>
|
||||||
#include <memory>
|
#include <memory>
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
@ -107,7 +108,7 @@ struct Connect : stages::Connect
|
|||||||
|
|
||||||
static GroupPlannerVector getPlanners() {
|
static GroupPlannerVector getPlanners() {
|
||||||
auto planner = std::make_shared<solvers::JointInterpolationPlanner>();
|
auto planner = std::make_shared<solvers::JointInterpolationPlanner>();
|
||||||
return { { "group1", planner }, { "group2", planner } };
|
return { { "group", planner }, { "eef_group", planner } };
|
||||||
}
|
}
|
||||||
|
|
||||||
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false)
|
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false)
|
||||||
@ -129,15 +130,6 @@ unsigned int ForwardMockup::id_ = 0;
|
|||||||
unsigned int BackwardMockup::id_ = 0;
|
unsigned int BackwardMockup::id_ = 0;
|
||||||
unsigned int Connect::id_ = 0;
|
unsigned int Connect::id_ = 0;
|
||||||
|
|
||||||
moveit::core::RobotModelConstPtr getModel() {
|
|
||||||
ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error);
|
|
||||||
moveit::core::RobotModelBuilder builder("robot", "base");
|
|
||||||
builder.addChain("base->a->b->c", "continuous");
|
|
||||||
builder.addGroupChain("base", "b", "group1");
|
|
||||||
builder.addGroupChain("b", "c", "group2");
|
|
||||||
return builder.build();
|
|
||||||
}
|
|
||||||
|
|
||||||
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
// https://github.com/ros-planning/moveit_task_constructor/issues/182
|
||||||
TEST(ConnectConnect, SuccSucc) {
|
TEST(ConnectConnect, SuccSucc) {
|
||||||
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs
|
||||||
|
|||||||
@ -47,8 +47,6 @@ public:
|
|||||||
};
|
};
|
||||||
|
|
||||||
TEST(Stage, registerCallbacks) {
|
TEST(Stage, registerCallbacks) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
|
|
||||||
GeneratorMockup g;
|
GeneratorMockup g;
|
||||||
g.init(getModel());
|
g.init(getModel());
|
||||||
|
|
||||||
@ -68,8 +66,6 @@ TEST(Stage, registerCallbacks) {
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST(ComputeIK, init) {
|
TEST(ComputeIK, init) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
|
|
||||||
auto g = std::make_unique<GeneratorMockup>();
|
auto g = std::make_unique<GeneratorMockup>();
|
||||||
stages::ComputeIK ik("ik", std::move(g));
|
stages::ComputeIK ik("ik", std::move(g));
|
||||||
moveit::core::RobotModelPtr robot_model = getModel();
|
moveit::core::RobotModelPtr robot_model = getModel();
|
||||||
@ -93,13 +89,11 @@ TEST(ComputeIK, init) {
|
|||||||
EXPECT_THROW(ik.init(robot_model), InitStageException);
|
EXPECT_THROW(ik.init(robot_model), InitStageException);
|
||||||
|
|
||||||
// valid group should not throw
|
// valid group should not throw
|
||||||
props.set("group", std::string("base_from_base_to_tip"));
|
props.set("group", std::string("group"));
|
||||||
EXPECT_NO_THROW(ik.init(robot_model));
|
EXPECT_NO_THROW(ik.init(robot_model));
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(ModifyPlanningScene, allowCollisions) {
|
TEST(ModifyPlanningScene, allowCollisions) {
|
||||||
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
|
|
||||||
|
|
||||||
auto s = std::make_unique<stages::ModifyPlanningScene>();
|
auto s = std::make_unique<stages::ModifyPlanningScene>();
|
||||||
std::string first = "foo", second = "boom";
|
std::string first = "foo", second = "boom";
|
||||||
s->allowCollisions(first, second, true);
|
s->allowCollisions(first, second, true);
|
||||||
@ -168,13 +162,13 @@ TEST(Connect, compatible) {
|
|||||||
|
|
||||||
// attached objects
|
// attached objects
|
||||||
other = scene->diff();
|
other = scene->diff();
|
||||||
attachObject(*scene, "object", "base_link", true);
|
attachObject(*scene, "object", "tip", true);
|
||||||
EXPECT_FALSE(connect.compatible(scene, other)) << "detached and attached object";
|
EXPECT_FALSE(connect.compatible(scene, other)) << "detached and attached object";
|
||||||
|
|
||||||
other = scene->diff();
|
other = scene->diff();
|
||||||
EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object";
|
EXPECT_TRUE(connect.compatible(scene, other)) << "identical scenes, attached object";
|
||||||
|
|
||||||
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
|
spawnObject(*other, "object", shape_msgs::SolidPrimitive::CYLINDER, { 0.1, 0, 0 });
|
||||||
attachObject(*other, "object", "base_link", true);
|
attachObject(*other, "object", "tip", true);
|
||||||
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
|
EXPECT_FALSE(connect.compatible(scene, other)) << "different pose";
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user