Skip to content

Commit f99d033

Browse files
rhaschkerhaschke
authored
Use RobotModelBuilder to simplify tests (#225)
* Simplify RobotModel definition using RobotModelBuilder * Silent RobotModel errors once in models.cpp
1 parent 8a913d8 commit f99d033

File tree

7 files changed

+23
-227
lines changed

7 files changed

+23
-227
lines changed

‎core/test/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -16,7 +16,7 @@ if (CATKIN_ENABLE_TESTING)
1616
target_link_libraries(${PROJECT_NAME}-test-container ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
1717

1818
catkin_add_gtest(${PROJECT_NAME}-test-serial test_serial.cpp)
19-
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_main)
19+
target_link_libraries(${PROJECT_NAME}-test-serial ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)
2020

2121
catkin_add_gtest(${PROJECT_NAME}-test-properties test_properties.cpp)
2222
target_link_libraries(${PROJECT_NAME}-test-properties ${PROJECT_NAME} gtest_main)
@@ -25,7 +25,7 @@ if (CATKIN_ENABLE_TESTING)
2525
target_link_libraries(${PROJECT_NAME}-test-cost_queue ${PROJECT_NAME} gtest_main)
2626

2727
catkin_add_gtest(${PROJECT_NAME}-test-interface_state test_interface_state.cpp)
28-
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_main)
28+
target_link_libraries(${PROJECT_NAME}-test-interface_state ${PROJECT_NAME} gtest_utils gtest_main)
2929

3030
catkin_add_gtest(${PROJECT_NAME}-test-cost_terms test_cost_terms.cpp)
3131
target_link_libraries(${PROJECT_NAME}-test-cost_terms ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main)

‎core/test/models.cpp‎

Lines changed: 11 additions & 178 deletions
Original file line numberDiff line numberDiff line change
@@ -1,187 +1,20 @@
11
#include "models.h"
2-
#include <moveit/robot_model/robot_model.h>
2+
#include <moveit/utils/robot_model_test_utils.h>
33
#include <moveit/robot_model_loader/robot_model_loader.h>
4-
#include <urdf_parser/urdf_parser.h>
54

65
using namespace moveit::core;
7-
namespace {
8-
9-
const std::string R_MODEL0 = "<?xml version=\"1.0\" ?>"
10-
"<robot name=\"one_robot\">"
11-
"<link name=\"base_link\">"
12-
" <inertial>"
13-
" <mass value=\"2.81\"/>"
14-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
15-
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
16-
" </inertial>"
17-
" <collision name=\"my_collision\">"
18-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
19-
" <geometry>"
20-
" <box size=\"1 2 1\" />"
21-
" </geometry>"
22-
" </collision>"
23-
" <visual>"
24-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
25-
" <geometry>"
26-
" <box size=\"1 2 1\" />"
27-
" </geometry>"
28-
" </visual>"
29-
"</link>"
30-
"<joint name=\"joint_a\" type=\"continuous\">"
31-
" <axis xyz=\"0 0 1\"/>"
32-
" <parent link=\"base_link\"/>"
33-
" <child link=\"link_a\"/>"
34-
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
35-
"</joint>"
36-
"<link name=\"link_a\">"
37-
" <inertial>"
38-
" <mass value=\"1.0\"/>"
39-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
40-
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
41-
" </inertial>"
42-
" <collision>"
43-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
44-
" <geometry>"
45-
" <box size=\"1 2 1\" />"
46-
" </geometry>"
47-
" </collision>"
48-
" <visual>"
49-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
50-
" <geometry>"
51-
" <box size=\"1 2 1\" />"
52-
" </geometry>"
53-
" </visual>"
54-
"</link>"
55-
"<joint name=\"joint_b\" type=\"fixed\">"
56-
" <parent link=\"link_a\"/>"
57-
" <child link=\"link_b\"/>"
58-
" <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
59-
"</joint>"
60-
"<link name=\"link_b\">"
61-
" <inertial>"
62-
" <mass value=\"1.0\"/>"
63-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
64-
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
65-
" </inertial>"
66-
" <collision>"
67-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
68-
" <geometry>"
69-
" <box size=\"1 2 1\" />"
70-
" </geometry>"
71-
" </collision>"
72-
" <visual>"
73-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
74-
" <geometry>"
75-
" <box size=\"1 2 1\" />"
76-
" </geometry>"
77-
" </visual>"
78-
"</link>"
79-
" <joint name=\"joint_c\" type=\"prismatic\">"
80-
" <axis xyz=\"1 0 0\"/>"
81-
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
82-
" <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
83-
"soft_upper_limit=\"0.089\"/>"
84-
" <parent link=\"link_b\"/>"
85-
" <child link=\"link_c\"/>"
86-
" <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
87-
" </joint>"
88-
"<link name=\"link_c\">"
89-
" <inertial>"
90-
" <mass value=\"1.0\"/>"
91-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
92-
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
93-
" </inertial>"
94-
" <collision>"
95-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
96-
" <geometry>"
97-
" <box size=\"1 2 1\" />"
98-
" </geometry>"
99-
" </collision>"
100-
" <visual>"
101-
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
102-
" <geometry>"
103-
" <box size=\"1 2 1\" />"
104-
" </geometry>"
105-
" </visual>"
106-
"</link>"
107-
" <joint name=\"mim_f\" type=\"prismatic\">"
108-
" <axis xyz=\"1 0 0\"/>"
109-
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
110-
" <parent link=\"link_c\"/>"
111-
" <child link=\"link_d\"/>"
112-
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
113-
" <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
114-
" </joint>"
115-
" <joint name=\"joint_f\" type=\"prismatic\">"
116-
" <axis xyz=\"1 0 0\"/>"
117-
" <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
118-
" <parent link=\"link_d\"/>"
119-
" <child link=\"link_e\"/>"
120-
" <origin rpy=\" 0.0 0.1 0.0 \" xyz=\"0.1 0.1 0 \"/>"
121-
" </joint>"
122-
"<link name=\"link_d\">"
123-
" <collision>"
124-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
125-
" <geometry>"
126-
" <box size=\"1 2 1\" />"
127-
" </geometry>"
128-
" </collision>"
129-
" <visual>"
130-
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
131-
" <geometry>"
132-
" <box size=\"1 2 1\" />"
133-
" </geometry>"
134-
" </visual>"
135-
"</link>"
136-
"<link name=\"link_e\">"
137-
" <collision>"
138-
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
139-
" <geometry>"
140-
" <box size=\"1 2 1\" />"
141-
" </geometry>"
142-
" </collision>"
143-
" <visual>"
144-
" <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
145-
" <geometry>"
146-
" <box size=\"1 2 1\" />"
147-
" </geometry>"
148-
" </visual>"
149-
"</link>"
150-
"</robot>";
151-
152-
const std::string S_MODEL0 =
153-
"<?xml version=\"1.0\" ?>"
154-
"<robot name=\"one_robot\">"
155-
"<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
156-
"<group name=\"base_from_joints\">"
157-
"<joint name=\"base_joint\"/>"
158-
"<joint name=\"joint_a\"/>"
159-
"<joint name=\"joint_c\"/>"
160-
"</group>"
161-
"<group name=\"mim_joints\">"
162-
"<joint name=\"joint_f\"/>"
163-
"<joint name=\"mim_f\"/>"
164-
"</group>"
165-
"<group name=\"base_with_subgroups\">"
166-
"<group name=\"base_from_base_to_tip\"/>"
167-
"<joint name=\"joint_c\"/>"
168-
"</group>"
169-
"<group name=\"base_from_base_to_tip\">"
170-
"<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
171-
"<joint name=\"base_joint\"/>"
172-
"</group>"
173-
"<group name=\"base_with_bad_subgroups\">"
174-
"<group name=\"error\"/>"
175-
"</group>"
176-
"<end_effector name=\"eef\" parent_link=\"link_b\" group=\"mim_joints\" parent_group=\"base_from_base_to_tip\"/>"
177-
"</robot>";
178-
} // namespace
1796

1807
RobotModelPtr getModel() {
181-
urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(R_MODEL0);
182-
srdf::ModelSharedPtr srdf_model(new srdf::Model());
183-
srdf_model->initString(*urdf_model, S_MODEL0);
184-
return RobotModelPtr(new RobotModel(urdf_model, srdf_model));
8+
// suppress RobotModel errors and warnings
9+
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME ".moveit_core.robot_model", ros::console::levels::Fatal);
10+
11+
// create dummy robot model
12+
moveit::core::RobotModelBuilder builder("robot", "base");
13+
builder.addChain("base->link1->link2->tip", "continuous");
14+
builder.addGroupChain("base", "link2", "group");
15+
builder.addGroupChain("link2", "tip", "eef_group");
16+
builder.addEndEffector("eef", "link2", "group", "eef_group");
17+
return builder.build();
18518
}
18619

18720
moveit::core::RobotModelPtr loadModel() {

‎core/test/test_container.cpp‎

Lines changed: 3 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -3,8 +3,8 @@
33
#include <moveit/task_constructor/task_p.h>
44
#include <moveit/task_constructor/stages/fixed_state.h>
55
#include <moveit/planning_scene/planning_scene.h>
6-
#include <moveit/utils/robot_model_test_utils.h>
76

7+
#include "models.h"
88
#include "gtest_value_printers.h"
99
#include <gtest/gtest.h>
1010
#include <initializer_list>
@@ -686,11 +686,7 @@ TEST(Task, move) {
686686
}
687687

688688
TEST(Task, reuse) {
689-
// create dummy robot model
690-
moveit::core::RobotModelBuilder builder("robot", "base");
691-
builder.addChain("base->a->b->c", "continuous");
692-
builder.addGroupChain("base", "c", "group");
693-
moveit::core::RobotModelConstPtr robot_model = builder.build();
689+
moveit::core::RobotModelConstPtr robot_model = getModel();
694690

695691
Task t("first");
696692
t.setRobotModel(robot_model);
@@ -725,13 +721,8 @@ TEST(Task, reuse) {
725721

726722
TEST(Task, timeout) {
727723
MOCK_ID = 0;
728-
// create dummy robot model
729-
moveit::core::RobotModelBuilder builder("robot", "base");
730-
builder.addChain("base->a->b->c", "continuous");
731-
builder.addGroupChain("base", "c", "group");
732-
733724
Task t;
734-
t.setRobotModel(builder.build());
725+
t.setRobotModel(getModel());
735726

736727
auto timeout = std::chrono::milliseconds(10);
737728
t.add(std::make_unique<GeneratorMockup>(100)); // allow up to 100 solutions spawned

‎core/test/test_cost_terms.cpp‎

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -172,7 +172,6 @@ class Standalone : public T
172172
};
173173

174174
TEST(CostTerm, SetLambdaCostTerm) {
175-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
176175
const moveit::core::RobotModelConstPtr robot{ getModel() };
177176

178177
Standalone<SerialContainer> container(robot);
@@ -198,7 +197,6 @@ TEST(CostTerm, SetLambdaCostTerm) {
198197
}
199198

200199
TEST(CostTerm, CostOverwrite) {
201-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
202200
const moveit::core::RobotModelConstPtr robot{ getModel() };
203201

204202
Standalone<SerialContainer> container(robot);
@@ -215,7 +213,6 @@ TEST(CostTerm, CostOverwrite) {
215213
}
216214

217215
TEST(CostTerm, StageTypes) {
218-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
219216
moveit::core::RobotModelPtr robot{ getModel() };
220217

221218
Standalone<SerialContainer> container(robot);
@@ -237,7 +234,6 @@ TEST(CostTerm, StageTypes) {
237234
}
238235

239236
TEST(CostTerm, PassThroughUsesCost) {
240-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
241237
moveit::core::RobotModelPtr robot{ getModel() };
242238
Standalone<stages::PassThrough> container(robot);
243239

@@ -253,7 +249,6 @@ TEST(CostTerm, PassThroughUsesCost) {
253249
}
254250

255251
TEST(CostTerm, PassThroughOverwritesCost) {
256-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
257252
moveit::core::RobotModelPtr robot{ getModel() };
258253
Standalone<stages::PassThrough> container(robot);
259254

@@ -271,7 +266,6 @@ TEST(CostTerm, PassThroughOverwritesCost) {
271266
}
272267

273268
TEST(CostTerm, PassThroughCanModifyCost) {
274-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
275269
moveit::core::RobotModelPtr robot{ getModel() };
276270
Standalone<stages::PassThrough> container(robot);
277271

@@ -287,7 +281,6 @@ TEST(CostTerm, PassThroughCanModifyCost) {
287281
}
288282

289283
TEST(CostTerm, CompositeSolutions) {
290-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
291284
Standalone<SerialContainer> container{ getModel() };
292285

293286
{
@@ -330,7 +323,6 @@ TEST(CostTerm, CompositeSolutions) {
330323
}
331324

332325
TEST(CostTerm, CompositeSolutionsContainerCost) {
333-
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
334326
Standalone<SerialContainer> container{ getModel() };
335327

336328
auto s1{ std::make_unique<ForwardMockup>() };

‎core/test/test_interface_state.cpp‎

Lines changed: 2 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
#include <moveit/task_constructor/storage.h>
2-
#include <moveit/utils/robot_model_test_utils.h>
2+
#include <moveit/task_constructor/stage_p.h>
33
#include <moveit/planning_scene/planning_scene.h>
44

5+
#include "models.h"
56
#include <memory>
67
#include <algorithm>
78
#include <iterator>
@@ -28,13 +29,6 @@ TEST(InterfaceStatePriority, compare) {
2829
EXPECT_TRUE(Prio(0, inf) > Prio(0, 0));
2930
}
3031

31-
moveit::core::RobotModelConstPtr getModel() {
32-
ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error);
33-
moveit::core::RobotModelBuilder builder("robot", "base");
34-
builder.addChain("base->tip", "continuous");
35-
return builder.build();
36-
}
37-
3832
using Prio = InterfaceState::Priority;
3933

4034
// Interface that also stores passed states

‎core/test/test_serial.cpp‎

Lines changed: 2 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@
55
#include <moveit/planning_scene/planning_scene.h>
66
#include <moveit/utils/robot_model_test_utils.h>
77

8+
#include "models.h"
89
#include <list>
910
#include <memory>
1011
#include <gtest/gtest.h>
@@ -107,7 +108,7 @@ struct Connect : stages::Connect
107108

108109
static GroupPlannerVector getPlanners() {
109110
auto planner = std::make_shared<solvers::JointInterpolationPlanner>();
110-
return { { "group1", planner }, { "group2", planner } };
111+
return { { "group", planner }, { "eef_group", planner } };
111112
}
112113

113114
Connect(std::initializer_list<double> costs = {}, bool enforce_sequential = false)
@@ -129,15 +130,6 @@ unsigned int ForwardMockup::id_ = 0;
129130
unsigned int BackwardMockup::id_ = 0;
130131
unsigned int Connect::id_ = 0;
131132

132-
moveit::core::RobotModelConstPtr getModel() {
133-
ros::console::set_logger_level("ros.moveit_core.robot_model", ros::console::levels::Error);
134-
moveit::core::RobotModelBuilder builder("robot", "base");
135-
builder.addChain("base->a->b->c", "continuous");
136-
builder.addGroupChain("base", "b", "group1");
137-
builder.addGroupChain("b", "c", "group2");
138-
return builder.build();
139-
}
140-
141133
// https://github.com/ros-planning/moveit_task_constructor/issues/182
142134
TEST(ConnectConnect, SuccSucc) {
143135
GeneratorMockup::id_ = Connect::id_ = 0; // reset IDs

0 commit comments

Comments
 (0)