moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
integrationtest_command_list_manager.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 <memory>
36#include <string>
37
38#include <gtest/gtest.h>
39
40#include <functional>
41#include <ros/ros.h>
42#include <ros/time.h>
43
47#include <moveit_msgs/DisplayTrajectory.h>
48#include <moveit_msgs/MotionPlanResponse.h>
49
50#include <tf2_eigen/tf2_eigen.hpp>
51
56
57#include "test_utils.h"
58
61
62const std::string ROBOT_DESCRIPTION_STR{ "robot_description" };
63const std::string EMPTY_VALUE{ "" };
64
65const std::string TEST_DATA_FILE_NAME("testdata_file_name");
66
68using namespace pilz_industrial_motion_planner;
70
71static std::string createManipulatorJointName(const size_t& joint_number)
72{
73 return std::string("prbt_joint_") + std::to_string(joint_number + 1);
74}
75
76static std::string createGripperJointName(const size_t& joint_number)
77{
78 switch (joint_number)
79 {
80 case 0:
81 return "prbt_gripper_finger_left_joint";
82 default:
83 break;
84 }
85 throw std::runtime_error("Could not create gripper joint name");
86}
87
88class IntegrationTestCommandListManager : public testing::Test
89{
90protected:
91 void SetUp() override;
92
93protected:
94 // ros stuff
95 ros::NodeHandle ph_{ "~" };
97 std::shared_ptr<pilz_industrial_motion_planner::CommandListManager> manager_;
98 planning_scene::PlanningScenePtr scene_;
99 planning_pipeline::PlanningPipelinePtr pipeline_;
100
101 std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> data_loader_;
102};
103
105{
106 // get necessary parameters
107 if (!robot_model_)
108 {
109 FAIL() << "Robot model could not be loaded.";
110 }
111
112 std::string test_data_file_name;
113 ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name));
114
115 // load the test data provider
117 std::make_unique<pilz_industrial_motion_planner_testutils::XmlTestdataLoader>(test_data_file_name, robot_model_);
118 ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider.";
119
120 // Define and set the current scene and manager test object
121 manager_ = std::make_shared<pilz_industrial_motion_planner::CommandListManager>(ph_, robot_model_);
122 scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
123 pipeline_ = std::make_shared<planning_pipeline::PlanningPipeline>(robot_model_, ph_);
124}
125
131TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping)
132{
133 auto nbr_ex = std::make_shared<NegativeBlendRadiusException>("");
134 EXPECT_EQ(nbr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
135
136 auto lbrnz_ex = std::make_shared<LastBlendRadiusNotZeroException>("");
137 EXPECT_EQ(lbrnz_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
138
139 auto sss_ex = std::make_shared<StartStateSetException>("");
140 EXPECT_EQ(sss_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE);
141
142 auto obr_ex = std::make_shared<OverlappingBlendRadiiException>("");
143 EXPECT_EQ(obr_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN);
144
145 auto pp_ex = std::make_shared<PlanningPipelineException>("");
146 EXPECT_EQ(pp_ex->getErrorCode(), moveit_msgs::msg::MoveItErrorCodes::FAILURE);
147}
148
170{
171 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
172 ASSERT_GE(seq.size(), 3u);
173 seq.erase(3, seq.size());
174 seq.setAllBlendRadiiToZero();
175
176 RobotTrajCont res123_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
177 EXPECT_EQ(res123_vec.size(), 1u);
178 EXPECT_GT(res123_vec.front()->getWayPointCount(), 0u);
179 EXPECT_TRUE(hasStrictlyIncreasingTime(res123_vec.front())) << "Time steps not strictly positively increasing";
180
181 ROS_INFO("step 2: only first segment");
182 moveit_msgs::msg::MotionSequenceRequest req_1;
183 req_1.items.resize(1);
184 req_1.items.at(0).req = seq.getCmd(0).toRequest();
185 req_1.items.at(0).blend_radius = 0.;
186 RobotTrajCont res1_vec{ manager_->solve(scene_, pipeline_, req_1) };
187 EXPECT_EQ(res1_vec.size(), 1u);
188 EXPECT_GT(res1_vec.front()->getWayPointCount(), 0u);
189 EXPECT_EQ(res1_vec.front()->getFirstWayPoint().getVariableCount(),
190 req_1.items.at(0).req.start_state.joint_state.name.size());
191
192 ROS_INFO("step 3: only second segment");
193 moveit_msgs::msg::MotionSequenceRequest req_2;
194 req_2.items.resize(1);
195 req_2.items.at(0).req = seq.getCmd(1).toRequest();
196 req_2.items.at(0).blend_radius = 0.;
197 RobotTrajCont res2_vec{ manager_->solve(scene_, pipeline_, req_2) };
198 EXPECT_EQ(res2_vec.size(), 1u);
199 EXPECT_GT(res2_vec.front()->getWayPointCount(), 0u);
200 EXPECT_EQ(res2_vec.front()->getFirstWayPoint().getVariableCount(),
201 req_2.items.at(0).req.start_state.joint_state.name.size());
202
203 ROS_INFO("step 4: only third segment");
204 moveit_msgs::msg::MotionSequenceRequest req_3;
205 req_3.items.resize(1);
206 req_3.items.at(0).req = seq.getCmd(2).toRequest();
207 req_3.items.at(0).blend_radius = 0.;
208 RobotTrajCont res3_vec{ manager_->solve(scene_, pipeline_, req_3) };
209 EXPECT_EQ(res3_vec.size(), 1u);
210 EXPECT_GT(res3_vec.front()->getWayPointCount(), 0u);
211 EXPECT_EQ(res3_vec.front()->getFirstWayPoint().getVariableCount(),
212 req_3.items.at(0).req.start_state.joint_state.name.size());
213
214 // durations for the different segments
215 auto t1_2_3 = res123_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1);
216 auto t1 = res1_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1);
217 auto t2 = res2_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1);
218 auto t3 = res3_vec.front()->getWayPointDurationFromStart(res123_vec.front()->getWayPointCount() - 1);
219 ROS_DEBUG_STREAM("total time: " << t1_2_3 << " t1:" << t1 << " t2:" << t2 << " t3:" << t3);
220 EXPECT_LT(fabs((t1_2_3 - t1 - t2 - t3)), 0.4);
221}
222
238TEST_F(IntegrationTestCommandListManager, concatSegmentsSelectiveBlending)
239{
240 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
241 ASSERT_GE(seq.size(), 3u);
242 seq.erase(3, seq.size());
243 seq.setAllBlendRadiiToZero();
244 seq.setBlendRadius(0, 0.1);
245 RobotTrajCont res1{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
246 EXPECT_EQ(res1.size(), 1u);
247 EXPECT_GT(res1.front()->getWayPointCount(), 0u);
248 EXPECT_TRUE(hasStrictlyIncreasingTime(res1.front())) << "Time steps not strictly positively increasing";
249
250 seq.setAllBlendRadiiToZero();
251 seq.setBlendRadius(1, 0.1);
252 RobotTrajCont res2{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
253 EXPECT_EQ(res2.size(), 1u);
254 EXPECT_GT(res2.front()->getWayPointCount(), 0u);
255 EXPECT_TRUE(hasStrictlyIncreasingTime(res2.front())) << "Time steps not strictly positively increasing";
256}
257
269{
270 Sequence seq{ data_loader_->getSequence("PtpPtpSequence") };
271 ASSERT_GE(seq.size(), 2u);
272 seq.setAllBlendRadiiToZero();
273
274 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
275 EXPECT_EQ(res_vec.size(), 1u);
276 EXPECT_GT(res_vec.front()->getWayPointCount(), 0u);
277 EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front()));
278}
279
291{
292 Sequence seq{ data_loader_->getSequence("PtpLinSequence") };
293 ASSERT_GE(seq.size(), 2u);
294 seq.setAllBlendRadiiToZero();
295
296 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
297 EXPECT_EQ(res_vec.size(), 1u);
298 EXPECT_GT(res_vec.front()->getWayPointCount(), 0u);
299 EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front()));
300}
301
313{
314 Sequence seq{ data_loader_->getSequence("LinPtpSequence") };
315 ASSERT_GE(seq.size(), 2u);
316 seq.setAllBlendRadiiToZero();
317
318 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
319 EXPECT_EQ(res_vec.size(), 1u);
320 EXPECT_GT(res_vec.front()->getWayPointCount(), 0u);
321 EXPECT_TRUE(hasStrictlyIncreasingTime(res_vec.front()));
322}
323
334{
335 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
336 ASSERT_EQ(seq.size(), 2u);
337 moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() };
338 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, req) };
339 EXPECT_EQ(res_vec.size(), 1u);
340 EXPECT_GT(res_vec.front()->getWayPointCount(), 0u);
341 EXPECT_EQ(res_vec.front()->getFirstWayPoint().getVariableCount(),
342 req.items.at(0).req.start_state.joint_state.name.size());
343
344 ros::NodeHandle nh;
345 ros::Publisher pub = nh.advertise<moveit_msgs::msg::DisplayTrajectory>("my_planned_path", 1);
346 ros::Duration duration(1.0); // wait to notify possible subscribers
347 duration.sleep();
348
349 moveit_msgs::msg::DisplayTrajectory display_trajectory;
350 moveit_msgs::msg::RobotTrajectory rob_traj_msg;
351 res_vec.front()->getRobotTrajectoryMsg(rob_traj_msg);
352 display_trajectory.trajectory.push_back(rob_traj_msg);
353 pub.publish(display_trajectory);
354}
355
356// ------------------
357// FAILURE cases
358// ------------------
359
370{
371 moveit_msgs::msg::MotionSequenceRequest empty_list;
372 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, empty_list) };
373 EXPECT_TRUE(res_vec.empty());
374}
375
386{
387 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
388 ASSERT_GE(seq.size(), 2u);
389 LinCart& lin{ seq.getCmd<LinCart>(0) };
390 lin.getGoalConfiguration().getPose().position.y = 2700;
391 EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), PlanningPipelineException);
392}
393
404{
405 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
406 ASSERT_GE(seq.size(), 2u);
407 const LinCart& lin{ seq.getCmd<LinCart>(0) };
408 moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() };
409 req.items.at(1).req.start_state = lin.getGoalConfiguration().toMoveitMsgsRobotState();
410 EXPECT_THROW(manager_->solve(scene_, pipeline_, req), StartStateSetException);
411}
412
424{
425 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
426 ASSERT_GE(seq.size(), 2u);
427 seq.setBlendRadius(0, -0.3);
428 EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), NegativeBlendRadiusException);
429}
430
441TEST_F(IntegrationTestCommandListManager, lastBlendingRadiusNonZero)
442{
443 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
444 ASSERT_EQ(seq.size(), 2u);
445 seq.setBlendRadius(1, 0.03);
446 EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), LastBlendRadiusNotZeroException);
447}
448
460TEST_F(IntegrationTestCommandListManager, blendRadiusGreaterThanSegment)
461{
462 Sequence seq{ data_loader_->getSequence("SimpleSequence") };
463 ASSERT_GE(seq.size(), 2u);
464 seq.setBlendRadius(0, 42.0);
465 EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), BlendingFailedException);
466}
467
480TEST_F(IntegrationTestCommandListManager, blendingRadiusOverlapping)
481{
482 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
483 ASSERT_GE(seq.size(), 3u);
484 seq.erase(3, seq.size());
485
486 RobotTrajCont res_valid_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
487 EXPECT_EQ(res_valid_vec.size(), 1u);
488 EXPECT_GT(res_valid_vec.front()->getWayPointCount(), 0u);
489
490 // calculate distance from first to second goal
491 const PtpJointCart& ptp{ seq.getCmd<PtpJointCart>(0) };
492 const CircInterimCart& circ{ seq.getCmd<CircInterimCart>(1) };
493 Eigen::Isometry3d p1, p2;
494 tf2::fromMsg(ptp.getGoalConfiguration().getPose(), p1);
495 tf2::fromMsg(circ.getGoalConfiguration().getPose(), p2);
496 auto distance = (p2.translation() - p1.translation()).norm();
497
498 seq.setBlendRadius(1, (distance - seq.getBlendRadius(0) + 0.01)); // overlapping radii
499 EXPECT_THROW(manager_->solve(scene_, pipeline_, seq.toRequest()), OverlappingBlendRadiiException);
500}
501
517{
518 Sequence seq{ data_loader_->getSequence("ComplexSequence") };
519 ASSERT_GE(seq.size(), 2u);
520 RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
521 EXPECT_EQ(res_single_vec.size(), 1u);
522 EXPECT_GT(res_single_vec.front()->getWayPointCount(), 0u);
523
524 moveit_msgs::msg::MotionSequenceRequest req{ seq.toRequest() };
525 // Create large request by making copies of the original sequence commands
526 // and adding them to the end of the original sequence.
527 const size_t n{ req.items.size() };
528 for (size_t i = 0; i < n; ++i)
529 {
530 moveit_msgs::msg::MotionSequenceItem item{ req.items.at(i) };
531 if (i == 0)
532 {
533 // Remove start state because only the first request
534 // is allowed to have a start state in a sequence.
535 item.req.start_state = moveit_msgs::msg::RobotState();
536 }
537 req.items.push_back(item);
538 }
539
540 RobotTrajCont res_n_vec{ manager_->solve(scene_, pipeline_, req) };
541 EXPECT_EQ(res_n_vec.size(), 1u);
542 EXPECT_GT(res_n_vec.front()->getWayPointCount(), 0u);
543
544 const double trajectory_time_1 =
545 res_single_vec.front()->getWayPointDurationFromStart(res_single_vec.front()->getWayPointCount() - 1);
546 const double trajectory_time_n =
547 res_n_vec.front()->getWayPointDurationFromStart(res_n_vec.front()->getWayPointCount() - 1);
548 double multiplicator = req.items.size() / n;
549 EXPECT_LE(trajectory_time_n, trajectory_time_1 * multiplicator);
550 EXPECT_GE(trajectory_time_n, trajectory_time_1 * multiplicator * 0.5);
551}
552
563{
564 Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") };
565 ASSERT_GE(seq.size(), 1u);
566 // Count the number of group changes in the given sequence
567 unsigned int num_groups{ 1 };
568 std::string last_group_name{ seq.getCmd(0).getPlanningGroup() };
569 for (size_t i = 1; i < seq.size(); ++i)
570 {
571 if (seq.getCmd(i).getPlanningGroup() != last_group_name)
572 {
573 ++num_groups;
574 last_group_name = seq.getCmd(i).getPlanningGroup();
575 }
576 }
577
578 RobotTrajCont res_single_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
579 EXPECT_EQ(res_single_vec.size(), num_groups);
580
581 for (const auto& res : res_single_vec)
582 {
583 EXPECT_GT(res->getWayPointCount(), 0u);
584 }
585}
586
593{
594 Sequence seq{ data_loader_->getSequence("PureGripperSequence") };
595 ASSERT_GE(seq.size(), 2u);
596 ASSERT_TRUE(seq.cmdIsOfType<Gripper>(0));
597 ASSERT_TRUE(seq.cmdIsOfType<Gripper>(1));
598
599 // Ensure that blending is requested for gripper commands.
600 seq.setBlendRadius(0, 1.0);
601 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
602 EXPECT_EQ(res_vec.size(), 1u);
603}
604
618TEST_F(IntegrationTestCommandListManager, TestGroupSpecificStartState)
619{
620 using std::placeholders::_1;
621
622 Sequence seq{ data_loader_->getSequence("ComplexSequenceWithGripper") };
623 ASSERT_GE(seq.size(), 4u);
624 seq.erase(4, seq.size());
625
626 Gripper& gripper{ seq.getCmd<Gripper>(0) };
627 gripper.getStartConfiguration().setCreateJointNameFunc([](size_t n) { return createGripperJointName(n); });
628 // By deleting the model we guarantee that the start state only consists
629 // of joints of the gripper group without the manipulator
630 gripper.getStartConfiguration().clearModel();
631
632 PtpJointCart& ptp{ seq.getCmd<PtpJointCart>(1) };
633 ptp.getStartConfiguration().setCreateJointNameFunc([](size_t n) { return createManipulatorJointName(n); });
634 // By deleting the model we guarantee that the start state only consists
635 // of joints of the manipulator group without the gripper
636 ptp.getStartConfiguration().clearModel();
637
638 RobotTrajCont res_vec{ manager_->solve(scene_, pipeline_, seq.toRequest()) };
639 EXPECT_GE(res_vec.size(), 1u);
640 EXPECT_GT(res_vec.front()->getWayPointCount(), 0u);
641}
642
647TEST_F(IntegrationTestCommandListManager, TestGetSolverTipFrameForSolverlessGroup)
648{
649 Gripper gripper_cmd{ data_loader_->getGripper("open_gripper") };
650 EXPECT_THROW(getSolverTipFrame(robot_model_->getJointModelGroup(gripper_cmd.getPlanningGroup())), NoSolverException);
651}
652
653int main(int argc, char** argv)
654{
655 ros::init(argc, argv, "integrationtest_command_list_manager");
656 testing::InitGoogleTest(&argc, argv);
657
658 ros::NodeHandle nh;
659
660 return RUN_ALL_TESTS();
661}
std::shared_ptr< pilz_industrial_motion_planner::CommandListManager > manager_
planning_pipeline::PlanningPipelinePtr pipeline_
std::unique_ptr< pilz_industrial_motion_planner_testutils::TestdataLoader > data_loader_
planning_interface::MotionPlanRequest toRequest() const override
Definition basecmd.h:112
Data class storing all information regarding a Circ command.
Definition circ.h:49
Data class storing all information regarding a linear command.
Definition lin.h:48
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
const moveit::core::RobotModelPtr & getModel() const
Get the constructed planning_models::RobotModel.
const std::string ROBOT_DESCRIPTION_STR
int main(int argc, char **argv)
const std::string TEST_DATA_FILE_NAME("testdata_file_name")
const std::string EMPTY_VALUE
TEST_F(IntegrationTestCommandListManager, TestExceptionErrorCodeMapping)
Checks that each derived MoveItErrorCodeException contains the correct error code.
std::vector< robot_trajectory::RobotTrajectoryPtr > RobotTrajCont
::testing::AssertionResult hasStrictlyIncreasingTime(const robot_trajectory::RobotTrajectoryPtr &trajectory)
Checks that every waypoint in the trajectory has a non-zero duration between itself and its predecess...