Skip to content
Snippets Groups Projects
Commit d93eb8cf authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

fixed tests

parents 8ee136ef 59f3a7ae
No related branches found
No related tags found
1 merge request!2Feature/public grasp side
...@@ -70,12 +70,20 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti ...@@ -70,12 +70,20 @@ target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUti
add_dependencies(grasp_service panda_grasping_generate_messages_cpp) add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
if(CATKIN_ENABLE_TESTING) if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED) 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) 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) target_link_libraries(tests_side_grasping ${catkin_LIBRARIES} GraspUtil)
add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp) add_dependencies(tests_side_grasping panda_grasping_generate_messages_cpp)
endif() endif()
...@@ -161,6 +161,13 @@ public: ...@@ -161,6 +161,13 @@ public:
bool placeFromSide(moveit::planning_interface::MoveGroupInterface &move_group, geometry_msgs::Pose &place_pose, 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); 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: private:
/** /**
......
...@@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> & ...@@ -24,10 +24,11 @@ void EnvironmentUtil::constructPlate(std::vector<moveit_msgs::CollisionObject> &
plate.primitive_poses[0].position.x = 0; plate.primitive_poses[0].position.x = 0;
plate.primitive_poses[0].position.y = 0; plate.primitive_poses[0].position.y = 0;
plate.primitive_poses[0].position.z = -0.1; plate.primitive_poses[0].position.z = -0.1;
plate.primitive_poses[0].orientation.w = 1;
plate.operation = plate.ADD; 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, 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> ...@@ -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.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.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].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; pick_support.operation = pick_support.ADD;
......
...@@ -40,8 +40,6 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -40,8 +40,6 @@ bool pickObject(panda_grasping::PickObject::Request &req,
if (!grasping_state::object_picked) { if (!grasping_state::object_picked) {
grasping_state::object_picked = true;
EnvironmentUtil env_util; EnvironmentUtil env_util;
GraspUtil grasp_util; GraspUtil grasp_util;
...@@ -79,6 +77,7 @@ bool pickObject(panda_grasping::PickObject::Request &req, ...@@ -79,6 +77,7 @@ bool pickObject(panda_grasping::PickObject::Request &req,
return false; return false;
} }
grasping_state::object_picked = true;
res.success = true; res.success = true;
return true; return true;
} }
......
...@@ -3,6 +3,8 @@ ...@@ -3,6 +3,8 @@
// //
#include "grasp_util.h" #include "grasp_util.h"
#include <tf2/transform_datatypes.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) { void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
posture.joint_names.resize(2); posture.joint_names.resize(2);
...@@ -333,3 +335,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g ...@@ -333,3 +335,21 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val); ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
return false; 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;
}
}
<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
<launch> <launch>
<include file="$(find panda_simulation)/launch/simulation.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" /> <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"/> <test test-name="tests_side_grasping" pkg="panda_grasping" type="tests_side_grasping" time-limit="3600.0"/>
......
//
// 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();
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment