Merge branch 'feature/onlytests' into feature/grasp_by_side

CMakeLists.txt

@@ -70,12 +70,20 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
 
 add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
 
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+
 if(CATKIN_ENABLE_TESTING)
     find_package(rostest REQUIRED)
+    add_rostest_gtest(tests_grasping test/grasping.test test/test.cpp)
+#    add_executable(tests_grasping test/test.cpp)
+    target_link_libraries(tests_grasping ${catkin_LIBRARIES})
+    add_dependencies(tests_grasping panda_grasping_generate_messages_cpp)
     add_rostest_gtest(tests_side_grasping test/side_grasping.test test/grasp_from_side_test.cpp)
     target_link_libraries(tests_side_grasping ${catkin_LIBRARIES} GraspUtil)
     add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp)
 endif()
 
-
-

src/grasping/environment_util.cpp

@@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
     plate.primitive_poses[0].position.x = 0;
     plate.primitive_poses[0].position.y = 0;
     plate.primitive_poses[0].position.z = -0.1;
+    plate.primitive_poses[0].orientation.w = 1;
 
     plate.operation = plate.ADD;
 
-    collision_objects.push_back(plate);
+//    collision_objects.push_back(plate);
 }
 
 void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject> &collision_objects, std::string id,
@@ -50,6 +51,7 @@ void EnvironmentUtil::constructSupport(std::vector<moveit_msgs::CollisionObject>
     pick_support.primitive_poses[0].position.x = object_to_pick_pose.position.x;
     pick_support.primitive_poses[0].position.y = object_to_pick_pose.position.y;
     pick_support.primitive_poses[0].position.z = (object_to_pick_pose.position.z - (object_z_dimension / 2) - 0.02);
+    pick_support.primitive_poses[0].orientation.w = 1; // TODO align to object
 
     pick_support.operation = pick_support.ADD;
 

src/grasping/grasp_service.cpp

@@ -40,8 +40,6 @@ bool pickObject(panda_grasping::PickObject::Request &req,
 
     if (!grasping_state::object_picked) {
 
-        grasping_state::object_picked = true;
-
         EnvironmentUtil env_util;
         GraspUtil grasp_util;
 
@@ -79,6 +77,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
             return false;
         }
 
+        grasping_state::object_picked = true;
         res.success = true;
         return true;
     }

test/grasping.test

+<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" respawn="true"/>
+
+    <test test-name="tests_grasping" pkg="panda_grasping" type="tests_grasping" time-limit="3600.0"/>
+</launch>
\ No newline at end of file

test/test.cpp

+//
+// Created by Johannes Mey on 22.11.20.
+//
+
+#include "ros/ros.h"
+
+#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
+#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>
+
+class Grasping : public ::testing::Test {
+protected:
+
+    void SetUp() override {
+        ros::NodeHandle n("panda_grasping");
+        pickClient = n.serviceClient<panda_grasping::PickObject>("PickObjectService");
+        placeClient = n.serviceClient<panda_grasping::PlaceObject>("PlaceObjectService");
+
+        // clean up scene
+        CleanupScene();
+    }
+
+
+    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 TearDown() override {}
+
+    ros::ServiceClient pickClient;
+    ros::ServiceClient placeClient;
+
+    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)
+    };
+
+    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};
+
+    std::vector<double> angles{
+            M_PI_2,
+            1,
+            -M_PI_4
+    };
+
+    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);
+
+            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");
+
+            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_F(Grasping, Grasping_WithoutRotation_Object_0_FromTo_01) {
+    auto object = objects[0];
+    pickAndPlace(object, points[0], 0, points[1], 0);
+}
+
+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);
+}
+
+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_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);
+}
+
+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);
+}
+
+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);
+}
+
+int main(int argc, char **argv) {
+    testing::InitGoogleTest(&argc, 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 <<<<<<<<<<<<<<<<");
+
+
+    return RUN_ALL_TESTS();
+}