moveit_cpp_test.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik LLC
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 LLC 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 /* Author: Jafar Abdi
36  Desc: Test the MoveItCpp and PlanningComponent interfaces
37 */
38 
39 // ROS
40 #include <ros/ros.h>
41 
42 // Testing
43 #include <gtest/gtest.h>
44 
45 // Main class
48 // Msgs
49 #include <geometry_msgs/PointStamped.h>
50 
51 namespace moveit_cpp
52 {
53 class MoveItCppTest : public ::testing::Test
54 {
55 public:
56  void SetUp() override
57  {
58  nh_ = ros::NodeHandle("/moveit_cpp_test");
59  moveit_cpp_ptr = std::make_shared<MoveItCpp>(nh_);
60  planning_component_ptr = std::make_shared<PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
61  robot_model_ptr = moveit_cpp_ptr->getRobotModel();
62  jmg_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
63 
64  target_pose1.header.frame_id = "panda_link0";
65  target_pose1.pose.orientation.w = 1.0;
66  target_pose1.pose.position.x = 0.28;
67  target_pose1.pose.position.y = -0.2;
68  target_pose1.pose.position.z = 0.5;
69 
70  start_pose.orientation.w = 1.0;
71  start_pose.position.x = 0.55;
72  start_pose.position.y = 0.0;
73  start_pose.position.z = 0.6;
74 
75  target_pose2.orientation.w = 1.0;
76  target_pose2.position.x = 0.55;
77  target_pose2.position.y = -0.05;
78  target_pose2.position.z = 0.8;
79  }
80 
81 protected:
83  MoveItCppPtr moveit_cpp_ptr;
84  PlanningComponentPtr planning_component_ptr;
85  moveit::core::RobotModelConstPtr robot_model_ptr;
87  const std::string PLANNING_GROUP = "panda_arm";
88  geometry_msgs::PoseStamped target_pose1;
89  geometry_msgs::Pose target_pose2;
90  geometry_msgs::Pose start_pose;
91 };
92 
93 // Test the current and the initial state of the Panda robot
94 TEST_F(MoveItCppTest, GetCurrentStateTest)
95 {
96  ros::Duration(1.0).sleep(); // Otherwise joint_states will result in an invalid robot state
97  auto robot_model = moveit_cpp_ptr->getRobotModel();
98  auto robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
99  EXPECT_TRUE(moveit_cpp_ptr->getCurrentState(robot_state, 0.0));
100  // Make sure the Panda robot is in "ready" state which is loaded from fake_controller.yaml
101  std::vector<double> joints_vals;
102  robot_state->copyJointGroupPositions(PLANNING_GROUP, joints_vals);
103  EXPECT_NEAR(joints_vals[0], 0.0, 0.001); // panda_joint1
104  EXPECT_NEAR(joints_vals[1], -0.785, 0.001); // panda_joint2
105  EXPECT_NEAR(joints_vals[2], 0.0, 0.001); // panda_joint3
106  EXPECT_NEAR(joints_vals[3], -2.356, 0.001); // panda_joint4
107  EXPECT_NEAR(joints_vals[4], 0.0, 0.001); // panda_joint5
108  EXPECT_NEAR(joints_vals[5], 1.571, 0.001); // panda_joint6
109  EXPECT_NEAR(joints_vals[6], 0.785, 0.001); // panda_joint7
110 }
111 
112 // Test the name of the planning group used by PlanningComponent for the Panda robot
113 TEST_F(MoveItCppTest, NameOfPlanningGroupTest)
114 {
115  EXPECT_STREQ(planning_component_ptr->getPlanningGroupName().c_str(), "panda_arm");
116 }
117 
118 // Test setting the start state of the plan to the current state of the robot
119 TEST_F(MoveItCppTest, TestSetStartStateToCurrentState)
120 {
121  planning_component_ptr->setStartStateToCurrentState();
122  planning_component_ptr->setGoal(target_pose1, "panda_link8");
123 
124  ASSERT_TRUE(static_cast<bool>(planning_component_ptr->plan()));
125  // TODO(JafarAbdi) adding testing to the soln state
126 }
127 
128 // Test setting the goal using geometry_msgs::PoseStamped and a robot's link name
129 TEST_F(MoveItCppTest, TestSetGoalFromPoseStamped)
130 {
131  planning_component_ptr->setGoal(target_pose1, "panda_link8");
132 
133  ASSERT_TRUE(static_cast<bool>(planning_component_ptr->plan()));
134 }
135 
136 // Test setting the plan start state using moveit::core::RobotState
137 TEST_F(MoveItCppTest, TestSetStartStateFromRobotState)
138 {
139  auto start_state = *(moveit_cpp_ptr->getCurrentState());
140  start_state.setFromIK(jmg_ptr, start_pose);
141 
142  planning_component_ptr->setStartState(start_state);
143  planning_component_ptr->setGoal(target_pose1, "panda_link8");
144 
145  ASSERT_TRUE(static_cast<bool>(planning_component_ptr->plan()));
146 }
147 
148 // Test settting the goal of the plan using a moveit::core::RobotState
149 TEST_F(MoveItCppTest, TestSetGoalFromRobotState)
150 {
151  auto target_state = *(moveit_cpp_ptr->getCurrentState());
152 
153  target_state.setFromIK(jmg_ptr, target_pose2);
154 
155  planning_component_ptr->setGoal(target_state);
156 
157  ASSERT_TRUE(static_cast<bool>(planning_component_ptr->plan()));
158 }
159 } // namespace moveit_cpp
160 
161 int main(int argc, char** argv)
162 {
163  testing::InitGoogleTest(&argc, argv);
164  ros::init(argc, argv, "moveit_cpp_test");
165 
167  spinner.start();
168 
169  int result = RUN_ALL_TESTS();
170 
171  return result;
172 }
moveit_cpp::MoveItCppTest::jmg_ptr
const moveit::core::JointModelGroup * jmg_ptr
Definition: moveit_cpp_test.cpp:150
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
moveit_cpp::MoveItCppTest::target_pose1
geometry_msgs::PoseStamped target_pose1
Definition: moveit_cpp_test.cpp:152
ros.h
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
ros::AsyncSpinner
moveit_cpp::MoveItCppTest::robot_model_ptr
moveit::core::RobotModelConstPtr robot_model_ptr
Definition: moveit_cpp_test.cpp:149
testing::Test
moveit_cpp::MoveItCppTest::nh_
ros::NodeHandle nh_
Definition: moveit_cpp_test.cpp:146
EXPECT_TRUE
#define EXPECT_TRUE(condition)
moveit_cpp::MoveItCppTest::moveit_cpp_ptr
MoveItCppPtr moveit_cpp_ptr
Definition: moveit_cpp_test.cpp:147
spinner
void spinner()
moveit_cpp
Definition: moveit_cpp.h:50
moveit_cpp::MoveItCppTest::PLANNING_GROUP
const std::string PLANNING_GROUP
Definition: moveit_cpp_test.cpp:151
moveit_cpp::MoveItCppTest::planning_component_ptr
PlanningComponentPtr planning_component_ptr
Definition: moveit_cpp_test.cpp:148
moveit_cpp::MoveItCppTest
Definition: moveit_cpp_test.cpp:85
EXPECT_STREQ
#define EXPECT_STREQ(expected, actual)
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
ASSERT_TRUE
#define ASSERT_TRUE(condition)
planning_component.h
moveit_cpp.h
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
moveit::core::JointModelGroup
moveit_cpp::MoveItCppTest::SetUp
void SetUp() override
Definition: moveit_cpp_test.cpp:120
ros::Duration::sleep
bool sleep() const
gtest.h
moveit_cpp::MoveItCppTest::target_pose2
geometry_msgs::Pose target_pose2
Definition: moveit_cpp_test.cpp:153
ros::Duration
main
int main(int argc, char **argv)
Definition: moveit_cpp_test.cpp:161
moveit_cpp::MoveItCppTest::start_pose
geometry_msgs::Pose start_pose
Definition: moveit_cpp_test.cpp:154
ros::NodeHandle
moveit_cpp::TEST_F
TEST_F(MoveItCppTest, GetCurrentStateTest)
Definition: moveit_cpp_test.cpp:126


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Nov 24 2022 03:32:50