fixed and completet test impl. for side grasps

test/grasp_from_side_test.cpp

@@ -14,10 +14,27 @@
  * Testing grasping from side.
  */
 class Grasping : public ::testing::Test {
+
 protected:
+
+    GraspUtil grasp_util;
+
+    geometry_msgs::Pose pick_table_pose;
+    geometry_msgs::Pose place_table_pose;
+    geometry_msgs::Pose obj_pose;
+    geometry_msgs::Pose place_pose;
+
+    tf2::Quaternion gripper_orientation;
+    tf2::Quaternion place_orientation;
+
     void SetUp() override {
 
         ros::NodeHandle n("panda_grasping");
+
+        gripper_orientation.setRPY(0, 0, -M_PI_4);
+        place_pose.position.z = 0.4 + 0.1 + 0.01; // table height + object height / 2 + buffer
+        place_orientation.setRPY(0, 0, M_PI / 2);
+
         CleanupScene();
     }
 
@@ -32,7 +49,8 @@ protected:
         ASSERT_EQ(planning_scene_interface.getAttachedObjects().size(), 0);
     }
 
-    void doTest(geometry_msgs::Pose &pick_table_pose, geometry_msgs::Pose &place_table_pose, geometry_msgs::Pose obj_pose, geometry_msgs::Pose place_pose){
+    void doTest(geometry_msgs::Pose &pick_pose, geometry_msgs::Pose &pick_table_pose, geometry_msgs::Pose &place_table_pose,
+            geometry_msgs::Pose &obj_pose, geometry_msgs::Pose &place_pose, geometry_msgs::Vector3 &approach_direction){
 
         moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
         moveit::planning_interface::MoveGroupInterface group("panda_arm");
@@ -47,7 +65,7 @@ protected:
         collision_objects[0].primitives[0].type = collision_objects[0].primitives[0].BOX;
         collision_objects[0].primitives[0].dimensions.resize(3);
         collision_objects[0].primitives[0].dimensions[0] = 0.2;
-        collision_objects[0].primitives[0].dimensions[1] = 0.4;
+        collision_objects[0].primitives[0].dimensions[1] = 0.2;
         collision_objects[0].primitives[0].dimensions[2] = 0.4;
         collision_objects[0].primitive_poses.resize(1);
         collision_objects[0].primitive_poses[0] = pick_table_pose;
@@ -59,7 +77,7 @@ protected:
         collision_objects[1].primitives.resize(1);
         collision_objects[1].primitives[0].type = collision_objects[1].primitives[0].BOX;
         collision_objects[1].primitives[0].dimensions.resize(3);
-        collision_objects[1].primitives[0].dimensions[0] = 0.4;
+        collision_objects[1].primitives[0].dimensions[0] = 0.2;
         collision_objects[1].primitives[0].dimensions[1] = 0.2;
         collision_objects[1].primitives[0].dimensions[2] = 0.4;
         collision_objects[1].primitive_poses.resize(1);
@@ -80,17 +98,11 @@ protected:
 
         planning_scene_interface.applyCollisionObjects(collision_objects);
 
-        pickAndPlace(obj_pose, planning_scene_interface, group, place_pose);
+        pickAndPlace(pick_pose, planning_scene_interface, group, place_pose, approach_direction);
     }
 
-    void pickAndPlace(geometry_msgs::Pose &obj_pose, moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
-            moveit::planning_interface::MoveGroupInterface &group, geometry_msgs::Pose &place_pose) {
-
-        tf2::Quaternion gripper_orientation;
-        gripper_orientation.setRPY(0, 0, -M_PI_4);
-
-        geometry_msgs::Vector3 approach_direction;
-        approach_direction.x = 1;
+    void pickAndPlace(geometry_msgs::Pose &pick_pose, moveit::planning_interface::PlanningSceneInterface &planning_scene_interface,
+            moveit::planning_interface::MoveGroupInterface &group, geometry_msgs::Pose &place_pose, geometry_msgs::Vector3 &approach_direction) {
 
         shape_msgs::SolidPrimitive object;
         object.type = shape_msgs::SolidPrimitive::CYLINDER;
@@ -100,27 +112,22 @@ protected:
 
         GraspUtil grasp_util;
 
-        geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
 
         ros::WallDuration(1.0).sleep();
 
-        grasp_util.pickFromSide(group, pick_pose, object, approach_direction,0.06, "table1", "object");
+        ASSERT_TRUE(grasp_util.pickFromSide(group, pick_pose, object, approach_direction,0.06, "table1", "object"));
 
         ros::WallDuration(1.0).sleep();
 
-        grasp_util.placeFromSide(group, place_pose, 0.06, "table2", "object", false);
+        ASSERT_TRUE(grasp_util.placeFromSide(group, place_pose, 0.06, "table2", "object", false));
     }
 };
 
+/**
+ * test from -x grasp
+*/
 TEST_F(Grasping, SIDE_GRASPING_1) {
 
-    ROS_ERROR("TEST 1");
-
-    geometry_msgs::Pose pick_table_pose;
-    geometry_msgs::Pose place_table_pose;
-    geometry_msgs::Pose obj_pose;
-    geometry_msgs::Pose place_pose;
-
     pick_table_pose.position.x = 0.5;
     pick_table_pose.position.y = -0.5;
     pick_table_pose.position.z = 0.2;
@@ -136,18 +143,108 @@ TEST_F(Grasping, SIDE_GRASPING_1) {
     obj_pose.position.z = 0.5;
     obj_pose.orientation.w = 1.0;
 
-    tf2::Quaternion orientation;
-    orientation.setRPY(0, 0, M_PI / 2);
-    place_pose.orientation = tf2::toMsg(orientation);
+    place_pose.orientation = tf2::toMsg(place_orientation);
     place_pose.position.x = place_table_pose.position.x;
     place_pose.position.y = place_table_pose.position.y;
-    place_pose.position.z = 0.4 + 0.1 + 0.01; // table height + object height / 2 + buffer
 
-    ROS_ERROR("TEST 2");
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.x = 1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from +x grasp
+*/
+TEST_F(Grasping, SIDE_GRASPING_2) {
 
-    doTest(pick_table_pose, place_table_pose, obj_pose, place_pose);
+    pick_table_pose.position.x = -0.5;
+    pick_table_pose.position.y = -0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = 0;
+    place_table_pose.position.y = 0.5;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = -0.5;
+    obj_pose.position.y = -0.5;
+    obj_pose.position.z = 0.5;
+    obj_pose.orientation.w = 1.0;
 
-    ROS_ERROR("TEST 3");
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.x = -1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromPlusXSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from -y grasp
+*/
+TEST_F(Grasping, SIDE_GRASPING_3) {
+
+    pick_table_pose.position.x = 0.5;
+    pick_table_pose.position.y = 0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = -0.5;
+    place_table_pose.position.y = 0;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = 0.5;
+    obj_pose.position.y = 0.5;
+    obj_pose.position.z = 0.5;
+    obj_pose.orientation.w = 1.0;
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.y = +1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromMinusYSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
+}
+
+/**
+ * test from +y grasp
+ */
+TEST_F(Grasping, SIDE_GRASPING_4) {
+
+    pick_table_pose.position.x = 0.5;
+    pick_table_pose.position.y = -0.5;
+    pick_table_pose.position.z = 0.2;
+    pick_table_pose.orientation.w = 1.0;
+
+    place_table_pose.position.x = -0.5;
+    place_table_pose.position.y = 0;
+    place_table_pose.position.z = 0.2;
+    place_table_pose.orientation.w = 1.0;
+
+    obj_pose.position.x = 0.5;
+    obj_pose.position.y = -0.5;
+    obj_pose.position.z = 0.5;
+    obj_pose.orientation.w = 1.0;
+
+    place_pose.orientation = tf2::toMsg(place_orientation);
+    place_pose.position.x = place_table_pose.position.x;
+    place_pose.position.y = place_table_pose.position.y;
+
+    geometry_msgs::Vector3 approach_direction;
+    approach_direction.y = -1;
+
+    geometry_msgs::Pose pick_pose = grasp_util.getPickFromPlusYSidePose(obj_pose, tf2::toMsg(gripper_orientation));
+    doTest(pick_pose, pick_table_pose, place_table_pose, obj_pose, place_pose, approach_direction);
 }
 
 int main(int argc, char **argv) {
@@ -155,6 +252,9 @@ int main(int argc, char **argv) {
     ros::init(argc, argv, "tests_side_grasping");
     ros::NodeHandle n("panda_grasping");
 
+    ros::AsyncSpinner spinner(4);
+    spinner.start();
+
     ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<");
     ros::Duration(3.0).sleep();
     ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> STARTING TEST AFTER INIT <<<<<<<<<<<<<<<<");

test/side_grasping.test

 <launch>
     <include file="$(find panda_simulation)/launch/simulation.launch"/>
 
-    <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+    <!--<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>-->
+
+
+    <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
 
     <test test-name="tests_side_grasping" pkg="panda_grasping" type="tests_side_grasping" time-limit="3600.0"/>
 </launch>