diff --git a/CMakeLists.txt b/CMakeLists.txt index 7176fa54eee2541f3de2fc2330b6dc1779e6f9b2..b3b288fba595ab0d1f95aef76a0cd4502a9f0b03 100644 --- a/CMakeLists.txt +++ b/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() - - diff --git a/include/grasp_util.h b/include/grasp_util.h index 325a826306b0463eb56c2cda7adad2491e62a7e9..1403f64d74fe93f9fcc654c2b81814167aec4b14 100644 --- a/include/grasp_util.h +++ b/include/grasp_util.h @@ -161,6 +161,13 @@ public: bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, double open_amount, std::string supporting_surface_id, std::string object_to_place_id, bool plan_only = false); + /** + * Homes the gripper (in case it was abused before and is in an inconsistent state) + * This method is blocking with a maximum runtime of ~30s + * @return true if homing succeeded, false otherwise + */ + static bool homeGripper(); + private: /** diff --git a/src/grasping/environment_util.cpp b/src/grasping/environment_util.cpp index 54766834b5e4fb2a625ddabc44a907940babeea2..6d8af3d85f7d18691629029c1c537c3379662ceb 100644 --- a/src/grasping/environment_util.cpp +++ b/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; diff --git a/src/grasping/grasp_service.cpp b/src/grasping/grasp_service.cpp index 7e27a2a4de28d4004599d4bd1a48a57efdc080fe..752d8eb1abcbaffe84196cb15220416aab7c2c1e 100644 --- a/src/grasping/grasp_service.cpp +++ b/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; } diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp index 7d9924561a137645e096641c6a4dbb3cb198b6fe..4698c3dc0902cc0680f1890f57e0987cb2f905f4 100644 --- a/src/grasping/grasp_util.cpp +++ b/src/grasping/grasp_util.cpp @@ -3,6 +3,8 @@ // #include "grasp_util.h" #include <tf2/transform_datatypes.h> +#include <franka_gripper/HomingAction.h> +#include <franka_gripper/HomingGoal.h> void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) { posture.joint_names.resize(2); @@ -333,3 +335,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); return false; } + +bool GraspUtil::homeGripper() { + actionlib::SimpleActionClient<franka_gripper::HomingAction> ac("/franka_gripper/homing", true); + if (!ac.waitForServer(ros::Duration(10.0))) { + ROS_ERROR("Gripper homing failed (action server not available)."); + return false; + } + franka_gripper::HomingGoal goal; + ac.sendGoal(goal); + if (ac.waitForResult(ros::Duration(20.0))) { + actionlib::SimpleClientGoalState state = ac.getState(); + ROS_INFO_STREAM("Homing finished: " << state.toString()); + return state == actionlib::SimpleClientGoalState::SUCCEEDED; + } else { + ROS_ERROR("Gripper homing failed (with timeout)."); + return false; + } +} diff --git a/test/grasping.test b/test/grasping.test new file mode 100644 index 0000000000000000000000000000000000000000..5765d69f50606ef7e5bc2165852b9177ca2aced9 --- /dev/null +++ b/test/grasping.test @@ -0,0 +1,14 @@ +<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 diff --git a/test/plan_visualizer.rviz b/test/plan_visualizer.rviz index 65ff1615893a1b2c90dc9ba320be53f74fd67eb0..2f631356378c3b3dc72a04abff35b9b9d5f1893f 100644 --- a/test/plan_visualizer.rviz +++ b/test/plan_visualizer.rviz @@ -283,4 +283,4 @@ Window Geometry: QMainWindow State: 000000ff00000000fd00000002000000000000013b000004e1fc0200000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000080000003640000017200fffffffb0000000800480065006c00700100000334000000e10000000000000000fb0000000a00560069006500770073000000003b000004e1000000000000000000000003000004fe0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720100000000000004fe0000010000ffffff000004fe000004e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Width: 1278 X: 0 - Y: 18 \ No newline at end of file + Y: 18 diff --git a/test/side_grasping.test b/test/side_grasping.test index 0ba7f0e537d6499492b036b5dbcfe79c214b8795..78444dedfb2f922ab9678e701639f327d69812d9 100644 --- a/test/side_grasping.test +++ b/test/side_grasping.test @@ -1,12 +1,5 @@ <launch> <include file="$(find panda_simulation)/launch/simulation.launch"/> - - <!-- <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>--> - - <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"/> diff --git a/test/test.cpp b/test/test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..185ea255f8b1f22361657f0486eb9be61c84435f --- /dev/null +++ b/test/test.cpp @@ -0,0 +1,182 @@ +// +// 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(); +}