moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
integrationtest_sequence_service_capability.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018 Pilz GmbH & Co. KG
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Pilz GmbH & Co. KG nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35#include <functional>
36#include <gtest/gtest.h>
37#include <iostream>
38#include <memory>
39#include <string>
40#include <vector>
41
46#include <moveit_msgs/Constraints.h>
47#include <moveit_msgs/GetMotionPlan.h>
48#include <moveit_msgs/JointConstraint.h>
49#include <moveit_msgs/MotionPlanResponse.h>
50#include <ros/ros.h>
51
54
55#include <moveit_msgs/GetMotionSequence.h>
56#include <moveit_msgs/MotionSequenceRequest.h>
58
59// Parameters from the node
60const std::string TEST_DATA_FILE_NAME("testdata_file_name");
61
63
64static std::string createJointName(const size_t& joint_number)
65{
66 return std::string("prbt_joint_") + std::to_string(joint_number + 1);
67}
68
69class IntegrationTestSequenceService : public ::testing::Test
70{
71protected:
72 void SetUp() override;
73
74protected:
75 ros::NodeHandle ph_{ "~" };
76 ros::ServiceClient client_;
77 robot_model::RobotModelPtr robot_model_;
78
81};
82
84{
85 // get necessary parameters
86 ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));
87
89 robot_model_ = model_loader.getModel();
90
91 data_loader_ = std::make_unique<XmlTestdataLoader>(test_data_file_name_, robot_model_);
92 ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider.";
93
94 ASSERT_TRUE(ros::service::waitForService(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME, ros::Duration(10)))
95 << "Service not available.";
96 ros::NodeHandle nh; // connect to service in global namespace, not in ph_
97 client_ = nh.serviceClient<moveit_msgs::GetMotionSequence>(pilz_industrial_motion_planner::SEQUENCE_SERVICE_NAME);
98}
99
111TEST_F(IntegrationTestSequenceService, TestSendingOfEmptySequence)
112{
113 moveit_msgs::msg::MotionSequenceRequest empty_list;
114
115 moveit_msgs::GetMotionSequence srv;
116 srv.request.request = empty_list;
117
118 ASSERT_TRUE(client_.call(srv));
119
120 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val) << "Planning failed.";
121 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
122}
123
137TEST_F(IntegrationTestSequenceService, TestDifferingGroupNames)
138{
139 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
140 MotionCmd& cmd{ seq.getCmd(1) };
141 cmd.setPlanningGroup("WrongGroupName");
142
143 moveit_msgs::GetMotionSequence srv;
144 srv.request.request = seq.toRequest();
145
146 ASSERT_TRUE(client_.call(srv));
147
148 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_GROUP_NAME, srv.response.response.error_code.val)
149 << "Planning should have failed but did not.";
150 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
151}
152
164TEST_F(IntegrationTestSequenceService, TestNegativeBlendRadius)
165{
166 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
167 seq.setBlendRadius(0, -1.0);
168
169 moveit_msgs::GetMotionSequence srv;
170 srv.request.request = seq.toRequest();
171
172 ASSERT_TRUE(client_.call(srv));
173
174 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val)
175 << "Planning should have failed but did not.";
176 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
177}
178
190TEST_F(IntegrationTestSequenceService, TestOverlappingBlendRadii)
191{
192 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
193 seq.setBlendRadius(0, 10 * seq.getBlendRadius(0));
194
195 moveit_msgs::GetMotionSequence srv;
196 srv.request.request = seq.toRequest();
197
198 ASSERT_TRUE(client_.call(srv));
199
200 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN, srv.response.response.error_code.val)
201 << "Incorrect error code";
202 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
203}
204
217{
218 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
219 seq.erase(2, seq.size());
220 seq.setBlendRadius(0, 10 * seq.getBlendRadius(seq.size() - 2));
221
222 moveit_msgs::GetMotionSequence srv;
223 srv.request.request = seq.toRequest();
224
225 ASSERT_TRUE(client_.call(srv));
226
227 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::FAILURE, srv.response.response.error_code.val)
228 << "Incorrect error code";
229 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
230}
231
245TEST_F(IntegrationTestSequenceService, TestSecondTrajInvalidStartState)
246{
247 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
248 moveit_msgs::msg::MotionSequenceRequest req_list{ seq.toRequest() };
249
250 // Set start state
251 using std::placeholders::_1;
252 JointConfiguration config{ "MyGroupName", { -1., 2., -3., 4., -5., 0. }, [](size_t n) { return createJointName(n); } };
253 req_list.items[1].req.start_state.joint_state = config.toSensorMsg();
254
255 moveit_msgs::GetMotionSequence srv;
256 srv.request.request = req_list;
257
258 ASSERT_TRUE(client_.call(srv));
259
260 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE, srv.response.response.error_code.val)
261 << "Incorrect error code.";
262 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
263}
264
278TEST_F(IntegrationTestSequenceService, TestFirstGoalNotReachable)
279{
280 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
281 PtpJointCart& cmd{ seq.getCmd<PtpJointCart>(0) };
282 cmd.getGoalConfiguration().getPose().position.y = 27;
283
284 moveit_msgs::GetMotionSequence srv;
285 srv.request.request = seq.toRequest();
286
287 ASSERT_TRUE(client_.call(srv));
288
289 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION, srv.response.response.error_code.val)
290 << "Incorrect error code.";
291 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
292}
293
306{
307 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
309
310 // Invalidate link name
311 CircInterimCart& circ{ seq.getCmd<CircInterimCart>(1) };
312 circ.getGoalConfiguration().setLinkName("InvalidLinkName");
313
314 moveit_msgs::GetMotionSequence srv;
315 srv.request.request = seq.toRequest();
316
317 ASSERT_TRUE(client_.call(srv));
318
319 EXPECT_NE(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val)
320 << "Incorrect error code.";
321 EXPECT_TRUE(srv.response.response.planned_trajectories.empty());
322}
323
336{
337 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
338 moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() };
339 // Make copy of sequence commands and add them to the end of sequence.
340 // Create large request by making copies of the original sequence commands
341 // and adding them to the end of the original sequence.
342 size_t n{ req.items.size() };
343 for (size_t i = 0; i < n; ++i)
344 {
345 moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) };
346 if (i == 0)
347 {
348 // Remove start state because only the first request
349 // is allowed to have a start state in a sequence.
350 item.req.start_state = moveit_msgs::msg::RobotState();
351 }
352 req.items.push_back(item);
353 }
354
355 moveit_msgs::GetMotionSequence srv;
356 srv.request.request = req;
357
358 ASSERT_TRUE(client_.call(srv));
359
360 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val)
361 << "Incorrect error code.";
362 EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u);
363 EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u)
364 << "Trajectory should contain points.";
365}
366
379TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithoutBlending)
380{
381 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
382
384
385 moveit_msgs::GetMotionSequence srv;
386 srv.request.request = seq.toRequest();
387
388 ASSERT_TRUE(client_.call(srv));
389
390 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val)
391 << "Incorrect error code.";
392 EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u);
393 EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u)
394 << "Trajectory should contain points.";
395}
396
409TEST_F(IntegrationTestSequenceService, TestComplexSequenceWithBlending)
410{
411 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
412
413 moveit_msgs::GetMotionSequence srv;
414 srv.request.request = seq.toRequest();
415
416 ASSERT_TRUE(client_.call(srv));
417
418 EXPECT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, srv.response.response.error_code.val)
419 << "Incorrect error code.";
420 EXPECT_EQ(srv.response.response.planned_trajectories.size(), 1u);
421 EXPECT_GT(srv.response.response.planned_trajectories.front().joint_trajectory.points.size(), 0u)
422 << "Trajectory should contain points.";
423}
424
425int main(int argc, char** argv)
426{
427 ros::init(argc, argv, "integrationtest_sequence_service_capability");
428 ros::NodeHandle nh;
429 testing::InitGoogleTest(&argc, argv);
430 return RUN_ALL_TESTS();
431}
Data class storing all information regarding a Circ command.
Definition circ.h:49
Class to define a robot configuration in space with the help of joint values.
Base class for commands storing all general information of a command.
Definition motioncmd.h:48
void setPlanningGroup(const std::string &planning_group)
Definition motioncmd.h:69
Data class storing all information regarding a Ptp command.
Definition ptp.h:48
Data class storing all information regarding a Sequence command.
Definition sequence.h:54
void setBlendRadius(const size_t index_cmd, const double blend_radius)
Definition sequence.h:130
void erase(const size_t start, const size_t end)
Deletes all commands from index 'start' to index 'end'.
Definition sequence.cpp:94
moveit_msgs::msg::MotionSequenceRequest toRequest() const
Definition sequence.cpp:67
const moveit::core::RobotModelPtr & getModel() const
Get the constructed planning_models::RobotModel.
int main(int argc, char **argv)
TEST_F(IntegrationTestSequenceService, TestSendingOfEmptySequence)
Test behavior of service when empty sequence is sent.
const std::string TEST_DATA_FILE_NAME("testdata_file_name")
std::unique_ptr< TestdataLoader > TestdataLoaderUPtr