moveit2
The MoveIt Motion Planning Framework for ROS 2.
integrationtest_command_planning.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 <gtest/gtest.h>
36 
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>
51 #include <ros/ros.h>
52 #include <tf2_eigen/tf2_eigen.hpp>
53 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
54 
55 #include "test_utils.h"
56 
57 const double EPSILON = 1.0e-6;
58 const std::string PLAN_SERVICE_NAME = "/plan_kinematic_path";
59 
60 // Parameters from the node
61 const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
62 const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance");
63 const std::string ORIENTATION_NORM_TOLERANCE("orientation_norm_tolerance");
64 const std::string PARAM_TARGET_LINK_NAME("target_link");
65 const std::string TEST_DATA_FILE_NAME("testdata_file_name");
66 
68 
75 class IntegrationTestCommandPlanning : public ::testing::Test
76 {
77 protected:
78  void SetUp() override;
79 
80 protected:
81  ros::NodeHandle ph_{ "~" };
82 
83  robot_model::RobotModelPtr robot_model_;
84 
85  double pose_norm_tolerance_, orientation_norm_tolerance_;
86  std::string planning_group_, target_link_, test_data_file_name_;
87 
88  std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> test_data_;
89 
90  unsigned int num_joints_{ 0 };
91 };
92 
94 {
95  // create robot model
97  robot_model_ = model_loader.getModel();
98 
99  // get the parameters
100  ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
101  ASSERT_TRUE(ph_.getParam(POSE_TRANSFORM_MATRIX_NORM_TOLERANCE, pose_norm_tolerance_));
102  ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
103  ASSERT_TRUE(ph_.getParam(ORIENTATION_NORM_TOLERANCE, orientation_norm_tolerance_));
104  ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));
105 
106  ASSERT_TRUE(ros::service::waitForService(PLAN_SERVICE_NAME, ros::Duration(testutils::DEFAULT_SERVICE_TIMEOUT)));
107 
108  // load the test data provider
109  test_data_ =
110  std::make_unique<pilz_industrial_motion_planner_testutils::XmlTestdataLoader>(test_data_file_name_, robot_model_);
111  ASSERT_NE(nullptr, test_data_) << "Failed to load test data by provider.";
112 
113  num_joints_ = robot_model_->getJointModelGroup(planning_group_)->getActiveJointModelNames().size();
114 }
115 
127 {
128  ros::NodeHandle node_handle("~");
129  auto ptp{ test_data_->getPtpJoint("Ptp1") };
130 
131  moveit_msgs::GetMotionPlan srv;
132  moveit_msgs::msg::MotionPlanRequest req = ptp.toRequest();
133  srv.request.motion_plan_request = req;
134 
135  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
136 
137  ASSERT_TRUE(client.call(srv));
138  const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response };
139 
140  // Check the result
141  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
142  trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory;
143 
144  EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames";
145  EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory";
146 
147  // Check that every point has position, velocity, acceleration
148  for (const auto& point : trajectory.points)
149  {
150  EXPECT_EQ(point.positions.size(), num_joints_);
151  EXPECT_EQ(point.velocities.size(), num_joints_);
152  EXPECT_EQ(point.accelerations.size(), num_joints_);
153  }
154 
155  for (size_t i = 0; i < num_joints_; ++i)
156  {
157  EXPECT_NEAR(trajectory.points.back().positions.at(i), req.goal_constraints.back().joint_constraints.at(i).position,
158  10e-10);
159  EXPECT_NEAR(trajectory.points.back().velocities.at(i), 0, 10e-10);
160  // EXPECT_NEAR(trajectory.points.back().accelerations.at(i), 0, 10e-10); //
161  // TODO what is expected
162  }
163 }
164 
176 {
177  ros::NodeHandle node_handle("~");
178 
179  PtpJointCart ptp{ test_data_->getPtpJointCart("Ptp1") };
180  ptp.getGoalConfiguration().setPoseTolerance(0.01);
181  ptp.getGoalConfiguration().setAngleTolerance(0.01);
182 
183  moveit_msgs::GetMotionPlan srv;
184  srv.request.motion_plan_request = ptp.toRequest();
185 
186  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
187 
188  ASSERT_TRUE(client.call(srv));
189  const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response };
190 
191  // Make sure the planning succeeded
192  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
193 
194  // Check the result
195  trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory;
196 
197  EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames";
198  EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory";
199 
200  // Check that every point has position, velocity, acceleration
201  for (const auto& point : trajectory.points)
202  {
203  EXPECT_EQ(point.positions.size(), num_joints_);
204  EXPECT_EQ(point.velocities.size(), num_joints_);
205  EXPECT_EQ(point.accelerations.size(), num_joints_);
206  }
207 
208  robot_state::RobotState rstate(robot_model_);
209  rstate.setToDefaultValues();
210  rstate.setJointGroupPositions(planning_group_, response.trajectory.joint_trajectory.points.back().positions);
211  Eigen::Isometry3d tf = rstate.getFrameTransform(target_link_);
212 
213  const geometry_msgs::Pose& expected_pose{ ptp.getGoalConfiguration().getPose() };
214 
215  EXPECT_NEAR(tf(0, 3), expected_pose.position.x, EPSILON);
216  EXPECT_NEAR(tf(1, 3), expected_pose.position.y, EPSILON);
217  EXPECT_NEAR(tf(2, 3), expected_pose.position.z, EPSILON);
218 
219  Eigen::Isometry3d exp_iso3d_pose;
220  tf2::fromMsg(expected_pose, exp_iso3d_pose);
221 
222  EXPECT_TRUE(Eigen::Quaterniond(tf.rotation()).isApprox(Eigen::Quaterniond(exp_iso3d_pose.rotation()), EPSILON));
223 }
224 
240 {
241  planning_interface::MotionPlanRequest req{ test_data_->getLinJoint("lin2").toRequest() };
242 
243  std::cout << "++++++++++" << '\n';
244  std::cout << "+ Step 1 +" << '\n';
245  std::cout << "++++++++++" << '\n';
246 
247  moveit_msgs::GetMotionPlan srv;
248  srv.request.motion_plan_request = req;
249 
250  ros::NodeHandle node_handle("~");
251  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
252 
253  ASSERT_TRUE(client.call(srv));
254  const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response };
255 
256  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
257 
258  std::cout << "++++++++++" << '\n';
259  std::cout << "+ Step 2 +" << '\n';
260  std::cout << "++++++++++" << '\n';
261 
262  ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_,
263  orientation_norm_tolerance_))
264  << "Goal not reached.";
265 
266  std::cout << "++++++++++" << '\n';
267  std::cout << "+ Step 3 +" << '\n';
268  std::cout << "++++++++++" << '\n';
269 
270  ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req,
271  pose_norm_tolerance_, orientation_norm_tolerance_))
272  << "Trajectory violates cartesian linearity.";
273 }
274 
291 {
292  ros::NodeHandle node_handle("~");
293  planning_interface::MotionPlanRequest req{ test_data_->getLinJointCart("lin2").toRequest() };
294 
295  std::cout << "++++++++++" << '\n';
296  std::cout << "+ Step 1 +" << '\n';
297  std::cout << "++++++++++" << '\n';
298 
299  moveit_msgs::GetMotionPlan srv;
300  srv.request.motion_plan_request = req;
301 
302  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
303 
304  ASSERT_TRUE(client.call(srv));
305  const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response };
306 
307  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
308 
309  std::cout << "++++++++++" << '\n';
310  std::cout << "+ Step 2 +" << '\n';
311  std::cout << "++++++++++" << '\n';
312 
313  ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_,
314  orientation_norm_tolerance_))
315  << "Goal not reached.";
316 
317  std::cout << "++++++++++" << '\n';
318  std::cout << "+ Step 3 +" << '\n';
319  std::cout << "++++++++++" << '\n';
320 
321  ASSERT_TRUE(testutils::checkCartesianLinearity(robot_model_, response.trajectory.joint_trajectory, req,
322  pose_norm_tolerance_, orientation_norm_tolerance_))
323  << "Trajectory violates cartesian linearity.";
324 }
325 
339 {
340  ros::NodeHandle node_handle("~");
341 
342  CircJointCenterCart circ{ test_data_->getCircJointCenterCart("circ1_center_2") };
343 
344  moveit_msgs::msg::MotionPlanRequest req{ circ.toRequest() };
345 
346  moveit_msgs::GetMotionPlan srv;
347  srv.request.motion_plan_request = req;
348 
349  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
350 
351  ASSERT_TRUE(client.call(srv));
352  const moveit_msgs::msg::MotionPlanResponse& response{ srv.response.motion_plan_response };
353 
354  // Check the result
355  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
356  trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory;
357 
358  EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames";
359  EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory";
360 
361  // Check that every point has position, velocity, acceleration
362  for (const auto& point : trajectory.points)
363  {
364  EXPECT_EQ(point.positions.size(), num_joints_);
365  EXPECT_EQ(point.velocities.size(), num_joints_);
366  EXPECT_EQ(point.accelerations.size(), num_joints_);
367  }
368 
369  // check goal is reached
370  ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_,
371  orientation_norm_tolerance_))
372  << "Goal not reached.";
373 
374  // check all waypoints are on the circle and SLERP
375  robot_state::RobotState waypoint_state(robot_model_);
376  Eigen::Isometry3d waypoint_pose;
377  double x_dist, y_dist, z_dist;
378 
379  const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() };
380 
381  CircCenterCart circ_cart{ test_data_->getCircCartCenterCart("circ1_center_2") };
382  const geometry_msgs::Pose& start_pose{ circ_cart.getStartConfiguration().getPose() };
383  const geometry_msgs::Pose& goal_pose{ circ_cart.getGoalConfiguration().getPose() };
384 
385  x_dist = aux_pose.position.x - start_pose.position.x;
386  y_dist = aux_pose.position.y - start_pose.position.y;
387  z_dist = aux_pose.position.z - start_pose.position.z;
388  double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist);
389  for (const auto& waypoint : trajectory.points)
390  {
391  waypoint_state.setToDefaultValues();
392  waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions);
393  waypoint_pose = waypoint_state.getFrameTransform(target_link_);
394 
395  // Calculate (and check) distance of current trajectory waypoint from circ
396  // center
397  x_dist = aux_pose.position.x - waypoint_pose(0, 3);
398  y_dist = aux_pose.position.y - waypoint_pose(1, 3);
399  z_dist = aux_pose.position.z - waypoint_pose(2, 3);
400  double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist);
401  EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle.";
402 
403  // Check orientation
404  Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d;
405  tf2::fromMsg(start_pose, start_pose_iso3d);
406  tf2::fromMsg(goal_pose, goal_pose_iso3d);
407  EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_));
408  }
409 }
410 
424 {
425  ros::NodeHandle node_handle("~");
426 
427  CircCenterCart circ{ test_data_->getCircCartCenterCart("circ1_center_2") };
428  moveit_msgs::msg::MotionPlanRequest req{ circ.toRequest() };
429  moveit_msgs::GetMotionPlan srv;
430  srv.request.motion_plan_request = req;
431 
432  ros::ServiceClient client = node_handle.serviceClient<moveit_msgs::GetMotionPlan>(PLAN_SERVICE_NAME);
433 
434  ASSERT_TRUE(client.call(srv));
435  const moveit_msgs::msg::MotionPlanResponse& response = srv.response.motion_plan_response;
436 
437  // Check the result
438  ASSERT_EQ(moveit_msgs::msg::MoveItErrorCodes::SUCCESS, response.error_code.val) << "Planning failed!";
439  trajectory_msgs::JointTrajectory trajectory = response.trajectory.joint_trajectory;
440 
441  EXPECT_EQ(trajectory.joint_names.size(), num_joints_) << "Wrong number of jointnames";
442  EXPECT_GT(trajectory.points.size(), 0u) << "There are no points in the trajectory";
443 
444  // Check that every point has position, velocity, acceleration
445  for (const auto& point : trajectory.points)
446  {
447  EXPECT_EQ(point.positions.size(), num_joints_);
448  EXPECT_EQ(point.velocities.size(), num_joints_);
449  EXPECT_EQ(point.accelerations.size(), num_joints_);
450  }
451 
452  // check goal is reached
453  ASSERT_TRUE(testutils::isGoalReached(robot_model_, response.trajectory.joint_trajectory, req, pose_norm_tolerance_,
454  orientation_norm_tolerance_))
455  << "Goal not reached.";
456 
457  // check all waypoints are on the cricle and SLERP
458  robot_state::RobotState waypoint_state(robot_model_);
459  Eigen::Isometry3d waypoint_pose;
460  double x_dist, y_dist, z_dist;
461 
462  const geometry_msgs::Pose& start_pose{ circ.getStartConfiguration().getPose() };
463  const geometry_msgs::Pose& aux_pose{ circ.getAuxiliaryConfiguration().getConfiguration().getPose() };
464  const geometry_msgs::Pose& goal_pose{ circ.getGoalConfiguration().getPose() };
465 
466  x_dist = aux_pose.position.x - start_pose.position.x;
467  y_dist = aux_pose.position.y - start_pose.position.y;
468  z_dist = aux_pose.position.z - start_pose.position.z;
469  double expected_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist);
470  for (const auto& waypoint : trajectory.points)
471  {
472  waypoint_state.setToDefaultValues();
473  waypoint_state.setJointGroupPositions(planning_group_, waypoint.positions);
474  waypoint_pose = waypoint_state.getFrameTransform(target_link_);
475 
476  // Calculate (and check) distance of current trajectory waypoint from circ
477  // center
478  x_dist = aux_pose.position.x - waypoint_pose(0, 3);
479  y_dist = aux_pose.position.y - waypoint_pose(1, 3);
480  z_dist = aux_pose.position.z - waypoint_pose(2, 3);
481  double actual_radius = sqrt(x_dist * x_dist + y_dist * y_dist + z_dist * z_dist);
482  EXPECT_NEAR(actual_radius, expected_radius, pose_norm_tolerance_) << "Trajectory way point is not on the circle.";
483 
484  // Check orientation
485  Eigen::Isometry3d start_pose_iso3d, goal_pose_iso3d;
486  tf2::fromMsg(start_pose, start_pose_iso3d);
487  tf2::fromMsg(goal_pose, goal_pose_iso3d);
488  EXPECT_TRUE(testutils::checkSLERP(start_pose_iso3d, goal_pose_iso3d, waypoint_pose, orientation_norm_tolerance_));
489  }
490 }
491 
492 int main(int argc, char** argv)
493 {
494  ros::init(argc, argv, "integrationtest_command_planning");
495  ros::NodeHandle nh; // For output via ROS_ERROR etc during test
496 
497  testing::InitGoogleTest(&argc, argv);
498  return RUN_ALL_TESTS();
499 }
std::unique_ptr< pilz_industrial_motion_planner_testutils::TestdataLoader > test_data_
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
const moveit::core::RobotModelPtr & getModel() const
Get the constructed planning_models::RobotModel.
const std::string PARAM_PLANNING_GROUP_NAME("planning_group")
int main(int argc, char **argv)
const std::string PARAM_TARGET_LINK_NAME("target_link")
const double EPSILON
TEST_F(IntegrationTestCommandPlanning, PtpJoint)
Tests if ptp motions with start & goal state given as joint configuration are executed correctly.
const std::string POSE_TRANSFORM_MATRIX_NORM_TOLERANCE("pose_norm_tolerance")
const std::string ORIENTATION_NORM_TOLERANCE("orientation_norm_tolerance")
const std::string PLAN_SERVICE_NAME
const std::string TEST_DATA_FILE_NAME("testdata_file_name")
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
bool isGoalReached(const trajectory_msgs::msg::JointTrajectory &trajectory, const std::vector< moveit_msgs::msg::JointConstraint > &goal, const double joint_position_tolerance, const double joint_velocity_tolerance=1.0e-6)
check if the goal given in joint space is reached Only the last point in the trajectory is veryfied.
Definition: test_utils.cpp:118
bool checkSLERP(const Eigen::Isometry3d &start_pose, const Eigen::Isometry3d &goal_pose, const Eigen::Isometry3d &wp_pose, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
check SLERP. The orientation should rotate around the same axis linearly.
Definition: test_utils.cpp:292
bool checkCartesianLinearity(const moveit::core::RobotModelConstPtr &robot_model, const trajectory_msgs::msg::JointTrajectory &trajectory, const planning_interface::MotionPlanRequest &req, const double translation_norm_tolerance, const double rot_axis_norm_tolerance, const double rot_angle_tolerance=10e-5)
Check that given trajectory is straight line.
Definition: test_utils.cpp:222