diff --git a/test/grasping.test b/test/grasping.test index 4625e3f870f202276c3cdccd1833a05efe8e745d..5765d69f50606ef7e5bc2165852b9177ca2aced9 100644 --- a/test/grasping.test +++ b/test/grasping.test @@ -1,12 +1,14 @@ <launch> - <include file="$(find panda_simulation)/launch/simulation.launch"/> + <include file="$(find panda_simulation)/launch/simulation.launch"> + <arg name="slow_simulation" value="false" /> + </include> <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false" args="-d $(find panda_grasping)/test/plan_visualizer.rviz" output="screen"> <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> </node> - <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/> + <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen" respawn="true"/> <test test-name="tests_grasping" pkg="panda_grasping" type="tests_grasping" time-limit="3600.0"/> </launch> \ No newline at end of file diff --git a/test/test.cpp b/test/test.cpp index 44b752c928db8b1ee60372d5d0f60ee8b2380872..185ea255f8b1f22361657f0486eb9be61c84435f 100644 --- a/test/test.cpp +++ b/test/test.cpp @@ -1,5 +1,3 @@ -#pragma clang diagnostic push -#pragma ide diagnostic ignored "cert-err58-cpp" // // Created by Johannes Mey on 22.11.20. // @@ -10,118 +8,164 @@ #include "tf2/LinearMath/Quaternion.h" #include "tf2/LinearMath/Vector3.h" +#include <moveit/planning_scene_interface/planning_scene_interface.h> +#include <moveit/move_group_interface/move_group_interface.h> + #include "panda_grasping/PickObject.h" #include "panda_grasping/PlaceObject.h" #include <gtest/gtest.h> -ros::ServiceClient pickClient; -ros::ServiceClient placeClient; +class Grasping : public ::testing::Test { +protected: -std::vector<tf2::Vector3> objects{ - tf2::Vector3(0.01, 0.1, 0.1), - tf2::Vector3(0.05, 0.1, 0.1), - tf2::Vector3(0.005, 0.1, 0.1) -}; + void SetUp() override { + ros::NodeHandle n("panda_grasping"); + pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService"); + placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService"); -std::vector<tf2::Vector3> points{ - tf2::Vector3(0.5, 0.3, 0.2), - tf2::Vector3(-0.5, -0.3, 0.3), - tf2::Vector3(0.5, 0.3, 0), - tf2::Vector3(0.4, 0.5, 0.4) -}; + // clean up scene + CleanupScene(); + } -std::vector<double> angles{ - M_PI_2, - 1, - -M_PI_4 -}; + static void CleanupScene() { + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + for (auto p : planning_scene_interface.getObjects()) { + p.second.operation = moveit_msgs::CollisionObject::REMOVE; + planning_scene_interface.applyCollisionObject(p.second); + } + ASSERT_EQ(planning_scene_interface.getObjects().size(), 0); + ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0); + } -void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) { - ROS_INFO_STREAM("starting test"); - tf2::Quaternion orientation; - { - orientation.setRPY(0, 0, fromAngle); + // void TearDown() override {} - panda_grasping::PickObject pickObjectSrv; - pickObjectSrv.request.dimensions = tf2::toMsg(size); - tf2::toMsg(from, pickObjectSrv.request.pose.position); - pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation); + ros::ServiceClient pickClient; + ros::ServiceClient placeClient; - ROS_INFO_STREAM("starting to pick"); + std::vector<tf2::Vector3> objects{ + tf2::Vector3(0.01, 0.1, 0.1), + tf2::Vector3(0.05, 0.1, 0.1), + tf2::Vector3(0.005, 0.1, 0.1) + }; - EXPECT_TRUE(pickClient.exists()); - EXPECT_TRUE(pickClient.call(pickObjectSrv)); - } - // wait a little while - ros::Duration(0.5).sleep(); - { - orientation.setRPY(0, 0, toAngle); + std::vector<tf2::Vector3> points{ + tf2::Vector3(0.5, 0.3, 0.2), + tf2::Vector3(-0.5, -0.3, 0.3), + tf2::Vector3(0.5, 0.3, 0.1), + tf2::Vector3(0.4, 0.5, 0.4), + + }; + + tf2::Vector3 nullLocation{-0.4, 0.5, 0}; - panda_grasping::PlaceObject placeObjectSrv; - placeObjectSrv.request.dimensions = tf2::toMsg(size); - tf2::toMsg(to, placeObjectSrv.request.pose.position); - placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation); + std::vector<double> angles{ + M_PI_2, + 1, + -M_PI_4 + }; - ROS_INFO_STREAM("starting to place"); + void pickAndPlace(tf2::Vector3 size, tf2::Vector3 from, double fromAngle, tf2::Vector3 to, double toAngle) { + // wait a little while + ros::Duration(0.5).sleep(); + ROS_INFO_STREAM("starting test"); + tf2::Quaternion orientation; + { + orientation.setRPY(0, 0, fromAngle); - EXPECT_TRUE(placeClient.exists()); - EXPECT_TRUE(placeClient.call(placeObjectSrv)); + panda_grasping::PickObject pickObjectSrv; + pickObjectSrv.request.dimensions = tf2::toMsg(size); + tf2::toMsg(from, pickObjectSrv.request.pose.position); + pickObjectSrv.request.pose.orientation = tf2::toMsg(orientation); + ROS_INFO_STREAM("starting to pick"); - ROS_INFO_STREAM("pick and place finished"); + ASSERT_TRUE(pickClient.exists()); + ASSERT_TRUE(pickClient.call(pickObjectSrv)); + } + { + orientation.setRPY(0, 0, toAngle); + + panda_grasping::PlaceObject placeObjectSrv; + placeObjectSrv.request.dimensions = tf2::toMsg(size); + tf2::toMsg(to, placeObjectSrv.request.pose.position); + placeObjectSrv.request.pose.orientation = tf2::toMsg(orientation); + + ROS_INFO_STREAM("starting to place"); + + ASSERT_TRUE(placeClient.exists()); + ASSERT_TRUE(placeClient.call(placeObjectSrv)); + + + ROS_INFO_STREAM("pick and place finished"); + } } -} +}; -TEST(Grasping, Grasping_WithoutRotation_Object_0) { +TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01) { auto object = objects[0]; - pickAndPlace(object, points[0], 0, points[1], 0); - pickAndPlace(object, points[2], 0, points[3], 0); } -TEST(Grasping, Grasping_WithoutRotation_Object_1) { - auto object = objects[1]; +TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_Null1) { + auto object = objects[0]; + pickAndPlace(object, nullLocation, 0, points[1], 0); +} +TEST_F(Grasping, Grasping_WithoutRotation_Object_1_FromTo_01) { + auto object = objects[1]; pickAndPlace(object, points[0], 0, points[1], 0); - pickAndPlace(object, points[2], 0, points[3], 0); } -TEST(Grasping, Grasping_WithoutRotation_Object_2) { +TEST_F(Grasping, Grasping_WithoutRotation_Object_2_FromTo_01) { auto object = objects[2]; + pickAndPlace(object, points[0], 0, points[1], 0); +} + +TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_1Null) { + auto object = objects[0]; + pickAndPlace(object, points[1], 0, nullLocation, 0); +} +TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_23) { + auto object = objects[0]; pickAndPlace(object, points[0], 0, points[1], 0); + CleanupScene(); pickAndPlace(object, points[2], 0, points[3], 0); } -TEST(Grasping, GraspingWithRotation_Object_0_Rotations_00) { +TEST_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01_12) { + auto object = objects[0]; + pickAndPlace(object, points[0], 0, points[1], 0); + CleanupScene(); + pickAndPlace(object, points[1], 0, points[2], 0); +} + +TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_00) { auto object = objects[0]; auto fromAngle = angles[0]; auto toAngle = angles[0]; pickAndPlace(object, points[0], fromAngle, points[1], toAngle); - pickAndPlace(object, points[2], fromAngle, points[3], toAngle); } -TEST(Grasping, GraspingWithRotation_Object_0_Rotations_01) { +TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_01) { auto object = objects[0]; auto fromAngle = angles[0]; auto toAngle = angles[1]; pickAndPlace(object, points[0], fromAngle, points[1], toAngle); - pickAndPlace(object, points[2], fromAngle, points[3], toAngle); } -TEST(Grasping, GraspingWithRotation_Object_0_Rotations_12) { +TEST_F(Grasping, GraspingWithRotation_Object_0_Rotations_12) { auto object = objects[0]; auto fromAngle = angles[1]; auto toAngle = angles[2]; pickAndPlace(object, points[0], fromAngle, points[1], toAngle); - pickAndPlace(object, points[2], fromAngle, points[3], toAngle); } int main(int argc, char **argv) { @@ -129,16 +173,10 @@ int main(int argc, char **argv) { ros::init(argc, argv, "grasping_test"); ros::NodeHandle n("panda_grasping"); - ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<"); ros::Duration(3.0).sleep(); ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<"); - pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService"); - placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService"); - return RUN_ALL_TESTS(); } - -#pragma clang diagnostic pop \ No newline at end of file