diff --git a/CMakeLists.txt b/CMakeLists.txt index d6d5ca231ddee8214e23b56d411d6a24d653a497..98513eb2df7f2c26a25a320ae8ec941e0263ad2a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -59,4 +59,17 @@ 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) +endif() diff --git a/test/grasping.test b/test/grasping.test new file mode 100644 index 0000000000000000000000000000000000000000..4625e3f870f202276c3cdccd1833a05efe8e745d --- /dev/null +++ b/test/grasping.test @@ -0,0 +1,12 @@ +<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> + + <node pkg="panda_grasping" type="grasp_service" name="grasp_service" output="screen"/> + + <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 new file mode 100644 index 0000000000000000000000000000000000000000..39129263ac47e7041802e815971ee4fe76b20300 --- /dev/null +++ b/test/plan_visualizer.rviz @@ -0,0 +1,296 @@ +Panels: + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: <Fixed Frame> + Value: true + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: panda_arm + Query Goal State: false + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 0.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + panda_hand: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_leftfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + panda_rightfinger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: false + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: panda_link0 + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz_visual_tools/KeyTool + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 2.9803097248077393 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.0864897966384888 + Y: 0.3387787342071533 + Z: 2.2351798634190345e-7 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.270203560590744 + Target Frame: panda_link0 + Value: XYOrbit (rviz) + Yaw: 0.42677485942840576 + Saved: ~ +Window Geometry: + Height: 1404 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000200000000000002a200000526fc0200000007fb000000100044006900730070006c006100790073010000003b000003e60000000000000000fb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000003b0000000b0000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000024d000003140000017200fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000004f0000000710000000000000000000000030000067f0000003ffc0100000001fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c006900640065007201000000000000067f0000010000ffffff0000067f000004e100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1663 + X: 0 + Y: 18 diff --git a/test/test.cpp b/test/test.cpp new file mode 100644 index 0000000000000000000000000000000000000000..44b752c928db8b1ee60372d5d0f60ee8b2380872 --- /dev/null +++ b/test/test.cpp @@ -0,0 +1,144 @@ +#pragma clang diagnostic push +#pragma ide diagnostic ignored "cert-err58-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 "panda_grasping/PickObject.h" +#include "panda_grasping/PlaceObject.h" + +#include <gtest/gtest.h> + +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), + tf2::Vector3(0.4, 0.5, 0.4) +}; + + +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) { + 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"); + + EXPECT_TRUE(pickClient.exists()); + EXPECT_TRUE(pickClient.call(pickObjectSrv)); + } + // wait a little while + ros::Duration(0.5).sleep(); + { + 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"); + + EXPECT_TRUE(placeClient.exists()); + EXPECT_TRUE(placeClient.call(placeObjectSrv)); + + + ROS_INFO_STREAM("pick and place finished"); + } +} + + +TEST(Grasping, Grasping_WithoutRotation_Object_0) { + 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]; + + pickAndPlace(object, points[0], 0, points[1], 0); + pickAndPlace(object, points[2], 0, points[3], 0); +} + +TEST(Grasping, Grasping_WithoutRotation_Object_2) { + auto object = objects[2]; + + pickAndPlace(object, points[0], 0, points[1], 0); + pickAndPlace(object, points[2], 0, points[3], 0); +} + +TEST(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) { + 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) { + 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) { + 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 <<<<<<<<<<<<<<<<"); + + + 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