From 78dc05819b6b49332b3eaa0d75c1dd4950eb837d Mon Sep 17 00:00:00 2001
From: SebastianEbert <sebastian.ebert@tu-dresden.de>
Date: Tue, 26 Oct 2021 17:51:35 +0200
Subject: [PATCH] fixed and completet test impl. for side grasps
---
test/grasp_from_side_test.cpp | 158 +++++++++++++++++++++++++++-------
test/side_grasping.test | 7 +-
2 files changed, 134 insertions(+), 31 deletions(-)
diff --git a/test/grasp_from_side_test.cpp b/test/grasp_from_side_test.cpp
index 23a201e..03f44e0 100644
--- a/test/grasp_from_side_test.cpp
+++ b/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;
+
+ 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) {
+
+ 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;
+
+ 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.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) {
- 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.5;
+ place_table_pose.position.y = 0;
+ place_table_pose.position.z = 0.2;
+ place_table_pose.orientation.w = 1.0;
- ROS_ERROR("TEST 3");
+ 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 <<<<<<<<<<<<<<<<");
diff --git a/test/side_grasping.test b/test/side_grasping.test
index 3644570..6b017a8 100644
--- a/test/side_grasping.test
+++ b/test/side_grasping.test
@@ -1,10 +1,13 @@
<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>
--
GitLab