moveit2
The MoveIt Motion Planning Framework for ROS 2.
move_group_interface_cpp_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, Tyler Weaver
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 PickNik Inc. nor the
18  * names of its contributors may be used to endorse or promote
19  * products derived from this software without specific prior
20  * written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Tyler Weaver, Boston Cleek */
37 
38 /* These integration tests are based on the tutorials for using move_group:
39  * https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html
40  */
41 
42 // C++
43 #include <string>
44 #include <vector>
45 #include <map>
46 #include <future>
47 
48 // ROS
49 #include <ros/ros.h>
50 
51 // The Testing Framework and Utils
52 #include <gtest/gtest.h>
53 
54 // MoveIt
59 
60 // TF2
61 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
62 #include <tf2_eigen/tf2_eigen.hpp>
63 
64 // 10um accuracy tested for position and orientation
65 constexpr double EPSILON = 1e-5;
66 
67 static const std::string PLANNING_GROUP = "panda_arm";
68 constexpr double PLANNING_TIME_S = 30.0;
69 constexpr double MAX_VELOCITY_SCALE = 1.0;
70 constexpr double MAX_ACCELERATION_SCALE = 1.0;
71 constexpr double GOAL_TOLERANCE = 1e-6;
72 
73 class MoveGroupTestFixture : public ::testing::Test
74 {
75 public:
76  void SetUp() override
77  {
78  nh_ = ros::NodeHandle("/move_group_interface_cpp_test");
79  move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(PLANNING_GROUP);
80 
81  // set velocity and acceleration scaling factors (full speed)
82  move_group_->setMaxVelocityScalingFactor(MAX_VELOCITY_SCALE);
83  move_group_->setMaxAccelerationScalingFactor(MAX_ACCELERATION_SCALE);
84 
85  // allow more time for planning
86  move_group_->setPlanningTime(PLANNING_TIME_S);
87 
88  // set the tolerance for the goals to be smaller than epsilon
89  move_group_->setGoalTolerance(GOAL_TOLERANCE);
90 
91  /* the tf buffer is not strictly needed,
92  but it's a simple way to add the codepaths to the tests */
93  psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description",
95  psm_->startSceneMonitor("/move_group/monitored_planning_scene");
96  psm_->requestPlanningSceneState();
97 
98  // give move_group_, planning_scene_interface_ and psm_ time to connect their topics
99  ros::Duration(0.5).sleep();
100  }
101 
102  // run updater() and ensure at least one geometry update was processed by the `move_group` node after doing so
103  void synchronizeGeometryUpdate(const std::function<void()>& updater)
104  {
105  SCOPED_TRACE("synchronizeGeometryUpdate");
106  std::promise<void> promise;
107  std::future<void> future = promise.get_future();
108  psm_->addUpdateCallback([this, &promise](planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType t) {
110  promise.set_value();
111  psm_->clearUpdateCallbacks();
112  });
113  updater();
114  // the updater must have triggered a geometry update, otherwise we can't be sure about the state of the scene anymore
115  ASSERT_EQ(future.wait_for(std::chrono::seconds(5)), std::future_status::ready);
116  }
117 
118  void planAndMoveToPose(const geometry_msgs::Pose& pose)
119  {
120  SCOPED_TRACE("planAndMoveToPose");
121  ASSERT_TRUE(move_group_->setJointValueTarget(pose));
122  planAndMove();
123  }
124 
125  void planAndMove()
126  {
127  SCOPED_TRACE("planAndMove");
129  ASSERT_EQ(move_group_->plan(my_plan), moveit::core::MoveItErrorCode::SUCCESS);
130  ASSERT_EQ(move_group_->move(), moveit::core::MoveItErrorCode::SUCCESS);
131  }
132 
133  void testEigenPose(const Eigen::Isometry3d& expected, const Eigen::Isometry3d& actual)
134  {
135  SCOPED_TRACE("testEigenPose");
136  std::stringstream ss;
137  ss << "expected: \n" << expected.matrix() << "\nactual: \n" << actual.matrix();
138  EXPECT_TRUE(actual.isApprox(expected, EPSILON)) << ss.str();
139  }
140 
141  void testPose(const Eigen::Isometry3d& expected_pose)
142  {
143  SCOPED_TRACE("testPose(const Eigen::Isometry3d&)");
144  // get the pose of the end effector link after the movement
145  geometry_msgs::PoseStamped actual_pose_stamped = move_group_->getCurrentPose();
146  Eigen::Isometry3d actual_pose;
147  tf2::fromMsg(actual_pose_stamped.pose, actual_pose);
148 
149  // compare to planned pose
150  testEigenPose(expected_pose, actual_pose);
151  }
152 
153  void testPose(const geometry_msgs::Pose& expected_pose_msg)
154  {
155  SCOPED_TRACE("testPose(const geometry_msgs::Pose&)");
156  Eigen::Isometry3d expected_pose;
157  tf2::fromMsg(expected_pose_msg, expected_pose);
158  testPose(expected_pose);
159  }
160 
161  void testJointPositions(const std::vector<double>& expected)
162  {
163  SCOPED_TRACE("testJointPositions");
164  const moveit::core::JointModelGroup* joint_model_group =
165  move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
166  std::vector<double> actual;
167  move_group_->getCurrentState()->copyJointGroupPositions(joint_model_group, actual);
168  ASSERT_EQ(expected.size(), actual.size());
169  for (size_t i = 0; i < actual.size(); ++i)
170  {
171  double delta = std::abs(expected[i] - actual[i]);
172  EXPECT_LT(delta, EPSILON) << "joint index: " << i << ", plan: " << expected[i] << ", result: " << actual[i];
173  }
174  }
175 
176 protected:
177  ros::NodeHandle nh_;
178  moveit::planning_interface::MoveGroupInterfacePtr move_group_;
180  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
181 };
182 
183 TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest)
184 {
185  SCOPED_TRACE("PathConstraintCollisionTest");
186 
188  // set a custom start state
189  // this simplifies planning for the orientation constraint below
190  geometry_msgs::Pose start_pose;
191  start_pose.orientation.w = 1.0;
192  start_pose.position.x = 0.3;
193  start_pose.position.y = 0.0;
194  start_pose.position.z = 0.6;
195  planAndMoveToPose(start_pose);
196 
198  // Test setting target pose with eigen and with geometry_msgs
199  geometry_msgs::Pose target_pose;
200  target_pose.orientation.w = 1.0;
201  target_pose.position.x = 0.3;
202  target_pose.position.y = -0.3;
203  target_pose.position.z = 0.6;
204 
205  // convert to eigen
206  Eigen::Isometry3d eigen_target_pose;
207  tf2::fromMsg(target_pose, eigen_target_pose);
208 
209  // set with eigen, get ros message representation
210  move_group_->setPoseTarget(eigen_target_pose);
211  geometry_msgs::PoseStamped set_target_pose = move_group_->getPoseTarget();
212  Eigen::Isometry3d eigen_set_target_pose;
213  tf2::fromMsg(set_target_pose.pose, eigen_set_target_pose);
214 
215  // expect that they are identical
216  testEigenPose(eigen_target_pose, eigen_set_target_pose);
217 
219  // create an orientation constraint
220  moveit_msgs::OrientationConstraint ocm;
221  ocm.link_name = move_group_->getEndEffectorLink();
222  ocm.header.frame_id = move_group_->getPlanningFrame();
223  ocm.orientation.w = 1.0;
224  ocm.absolute_x_axis_tolerance = 0.1;
225  ocm.absolute_y_axis_tolerance = 0.1;
226  ocm.absolute_z_axis_tolerance = 0.1;
227  ocm.weight = 1.0;
228  moveit_msgs::Constraints test_constraints;
229  test_constraints.orientation_constraints.push_back(ocm);
230  move_group_->setPathConstraints(test_constraints);
231 
233  // plan and move
234  planAndMove();
235 
236  // get the pose after the movement
237  testPose(eigen_target_pose);
238 
239  // clear path constraints
240  move_group_->clearPathConstraints();
241 }
242 
243 TEST_F(MoveGroupTestFixture, ModifyPlanningSceneAsyncInterfaces)
244 {
246  // Define a collision object ROS message.
247  moveit_msgs::CollisionObject collision_object;
248  collision_object.header.frame_id = move_group_->getPlanningFrame();
249 
250  // The id of the object is used to identify it.
251  collision_object.id = "box1";
252 
253  // Define a box to add to the world.
254  shape_msgs::SolidPrimitive primitive;
255  primitive.type = primitive.BOX;
256  primitive.dimensions.resize(3);
257  primitive.dimensions[0] = 0.1;
258  primitive.dimensions[1] = 1.0;
259  primitive.dimensions[2] = 1.0;
260 
261  // Define a pose for the box (specified relative to frame_id)
262  geometry_msgs::Pose box_pose;
263  box_pose.orientation.w = 1.0;
264  box_pose.position.x = 0.5;
265  box_pose.position.y = 0.0;
266  box_pose.position.z = 0.5;
267 
268  collision_object.primitives.push_back(primitive);
269  collision_object.primitive_poses.push_back(box_pose);
270  collision_object.operation = collision_object.ADD;
271 
272  std::vector<moveit_msgs::CollisionObject> collision_objects;
273  collision_objects.push_back(collision_object);
274 
275  // Now, let's add the collision object into the world
276  synchronizeGeometryUpdate([&]() { planning_scene_interface_.addCollisionObjects(collision_objects); });
277 
278  // attach and detach collision object
279  synchronizeGeometryUpdate([&]() { EXPECT_TRUE(move_group_->attachObject(collision_object.id)); });
280  EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(1));
281  synchronizeGeometryUpdate([&]() { EXPECT_TRUE(move_group_->detachObject(collision_object.id)); });
282  EXPECT_EQ(planning_scene_interface_.getAttachedObjects().size(), std::size_t(0));
283 
284  // remove object from world
285  const std::vector<std::string> object_ids = { collision_object.id };
286  EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(1));
287  synchronizeGeometryUpdate([&]() { planning_scene_interface_.removeCollisionObjects(object_ids); });
288  EXPECT_EQ(planning_scene_interface_.getObjects().size(), std::size_t(0));
289 }
290 
292 {
293  SCOPED_TRACE("CartPathTest");
294 
295  // Plan from current pose
296  const geometry_msgs::PoseStamped start_pose = move_group_->getCurrentPose();
297 
298  std::vector<geometry_msgs::Pose> waypoints;
299  waypoints.push_back(start_pose.pose);
300 
301  geometry_msgs::Pose target_waypoint = start_pose.pose;
302  target_waypoint.position.z -= 0.2;
303  waypoints.push_back(target_waypoint); // down
304 
305  target_waypoint.position.y -= 0.2;
306  waypoints.push_back(target_waypoint); // right
307 
308  target_waypoint.position.z += 0.2;
309  target_waypoint.position.y += 0.2;
310  target_waypoint.position.x -= 0.2;
311  waypoints.push_back(target_waypoint); // up and left
312 
313  moveit_msgs::RobotTrajectory trajectory;
314  const auto jump_threshold = 0.0;
315  const auto eef_step = 0.01;
316 
317  // test below is meaningless if Cartesian planning did not succeed
318  ASSERT_GE(EPSILON + move_group_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory), 1.0);
319 
320  // Execute trajectory
321  EXPECT_EQ(move_group_->execute(trajectory), moveit::core::MoveItErrorCode::SUCCESS);
322 
323  // get the pose after the movement
324  testPose(target_waypoint);
325 }
326 
327 TEST_F(MoveGroupTestFixture, JointSpaceGoalTest)
328 {
329  SCOPED_TRACE("JointSpaceGoalTest");
330 
331  // Next get the current set of joint values for the group.
332  std::vector<double> plan_joint_positions;
333  move_group_->getCurrentState()->copyJointGroupPositions(
334  move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP), plan_joint_positions);
335 
336  // Now, let's modify the joint positions. (radians)
337  ASSERT_EQ(plan_joint_positions.size(), std::size_t(7));
338  plan_joint_positions = { 1.2, -1.0, -0.1, -2.4, 0.0, 1.5, 0.6 };
339  move_group_->setJointValueTarget(plan_joint_positions);
340 
341  // plan and move
342  planAndMove();
343 
344  // test that we moved to the expected joint positions
345  testJointPositions(plan_joint_positions);
346 }
347 
348 int main(int argc, char** argv)
349 {
350  ros::init(argc, argv, "move_group_interface_cpp_test");
351  testing::InitGoogleTest(&argc, argv);
352 
353  ros::AsyncSpinner spinner(1);
354  spinner.start();
355 
356  int result = RUN_ALL_TESTS();
357  return result;
358 }
void synchronizeGeometryUpdate(const std::function< void()> &updater)
void testJointPositions(const std::vector< double > &expected)
void testPose(const geometry_msgs::Pose &expected_pose_msg)
void testPose(const Eigen::Isometry3d &expected_pose)
void testEigenPose(const Eigen::Isometry3d &expected, const Eigen::Isometry3d &actual)
moveit::planning_interface::MoveGroupInterfacePtr move_group_
moveit::planning_interface::PlanningSceneInterface planning_scene_interface_
planning_scene_monitor::PlanningSceneMonitorPtr psm_
void planAndMoveToPose(const geometry_msgs::Pose &pose)
@ UPDATE_GEOMETRY
The geometry of the scene was updated. This includes receiving new octomaps, collision objects,...
constexpr double PLANNING_TIME_S
constexpr double MAX_ACCELERATION_SCALE
int main(int argc, char **argv)
constexpr double GOAL_TOLERANCE
constexpr double EPSILON
constexpr double MAX_VELOCITY_SCALE
TEST_F(MoveGroupTestFixture, PathConstraintCollisionTest)
std::shared_ptr< tf2_ros::Buffer > getSharedTF()
The representation of a motion plan (as ROS messasges)