Skip to content
Snippets Groups Projects
Commit a111c182 authored by Johannes Mey's avatar Johannes Mey
Browse files

first try at tests

parent 82a0bfb5
No related branches found
No related tags found
1 merge request!2Feature/public grasp side
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 13
− 0
View file @ a111c182
......@@ -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()
test/grasping.test 0 → 100644
+ 12
− 0
View file @ a111c182
<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
test/plan_visualizer.rviz 0 → 100644
+ 296
− 0
View file @ a111c182
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
test/test.cpp 0 → 100644
+ 144
− 0
View file @ a111c182
#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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment