diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98513eb2df7f2c26a25a320ae8ec941e0263ad2a..9f09e30600fb699a80ec9f9b61ad8321e00f9274 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,13 +8,14 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_C
# Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
# catkin packages
find_package(catkin REQUIRED
- COMPONENTS
- roscpp
- std_msgs
- tf2
- moveit_core
- moveit_ros_planning_interface
- message_generation)
+ COMPONENTS
+ roscpp
+ std_msgs
+ tf2
+ moveit_core
+ moveit_ros_planning_interface
+ franka_gripper
+ message_generation)
## Generate services in the 'srv' folder
add_service_files(
@@ -35,10 +36,10 @@ generate_messages(
# ################################################################################################################################
# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
catkin_package(CATKIN_DEPENDS
- moveit_core
- moveit_visual_tools
- moveit_ros_planning_interface
- pluginlib
+ moveit_core
+ moveit_visual_tools
+ moveit_ros_planning_interface
+ pluginlib
)
# ################################################################################################################################
@@ -50,11 +51,13 @@ include_directories(${catkin_INCLUDE_DIRS})
add_library(EnvironmentUtil src/grasping/environment_util.cpp src/grasping/environment_util.h)
add_library(GraspUtil src/grasping/grasp_util.cpp src/grasping/grasp_util.h)
+add_library(FrankaGripperUtil src/grasping/franka_gripper_util.cpp src/grasping/franka_gripper_util.h)
add_executable(grasp_service src/grasping/grasp_service.cpp)
target_link_libraries(EnvironmentUtil ${catkin_LIBRARIES})
target_link_libraries(GraspUtil ${catkin_LIBRARIES})
+target_link_libraries(FrankaGripperUtil ${catkin_LIBRARIES})
target_link_libraries(grasp_service ${catkin_LIBRARIES} EnvironmentUtil GraspUtil)
add_dependencies(grasp_service panda_grasping_generate_messages_cpp)
diff --git a/package.xml b/package.xml
index ef381ee78ea2ec3a41e042fe8a28870cf7027e33..94db6d09fcecffeab978784e6d6c1d115ae1e851 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<package format="2">
<name>panda_grasping</name>
- <version>0.0.0</version>
+ <version>0.1.0</version>
<description>The panda_grasping package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
@@ -29,6 +29,7 @@
<depend>moveit_visual_tools</depend>
<depend>robot_state_publisher</depend>
<depend>tf2</depend>
+ <depend>franka_gripper</depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>roscpp</exec_depend>
diff --git a/src/grasping/franka_gripper_util.cpp b/src/grasping/franka_gripper_util.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..08a458f907efc3342a5d228cfc5d7cf439b897b1
--- /dev/null
+++ b/src/grasping/franka_gripper_util.cpp
@@ -0,0 +1,35 @@
+//
+// Created by jm on 03.12.20.
+//
+
+#include "franka_gripper_util.h"
+
+// Actionlib
+#include <actionlib/client/simple_action_client.h>
+
+// Franka
+#include <franka_gripper/MoveAction.h>
+
+void FrankaGripperUtil::resetGripperForNextAction() {
+
+ actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
+
+ ROS_INFO("Waiting for reset action server to start.");
+ ac.waitForServer();
+
+ ROS_INFO("Reset action server started, sending goal.");
+ franka_gripper::MoveGoal goal;
+ goal.speed = 0.08;
+ goal.width = 0.0;
+ ac.sendGoal(goal);
+
+ //wait for the action to return
+ bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
+
+ if (finished_before_timeout) {
+ actionlib::SimpleClientGoalState state = ac.getState();
+ ROS_INFO("Reset action finished: %s", state.toString().c_str());
+ } else {
+ ROS_INFO("Reset action did not finish before the time out.");
+ }
+}
diff --git a/src/grasping/franka_gripper_util.h b/src/grasping/franka_gripper_util.h
new file mode 100644
index 0000000000000000000000000000000000000000..a58376361a40736406203b1ac6bf220d80b4257e
--- /dev/null
+++ b/src/grasping/franka_gripper_util.h
@@ -0,0 +1,19 @@
+//
+// Created by jm on 03.12.20.
+//
+
+#ifndef PUBLIC_FRANKA_GRIPPER_UTIL_H
+#define PUBLIC_FRANKA_GRIPPER_UTIL_H
+
+
+class FrankaGripperUtil {
+
+ /**
+ * resets the real robot, to allow correct future movements and grasps
+ */
+ void resetGripperForNextAction();
+
+};
+
+
+#endif //PUBLIC_FRANKA_GRIPPER_UTIL_H
diff --git a/src/grasping/grasp_util.cpp b/src/grasping/grasp_util.cpp
index d3058621b0d8d115d4de59a8e959494dd94221b3..64f79e29e3df4befb46ec8c513ade03e6b08fb31 100644
--- a/src/grasping/grasp_util.cpp
+++ b/src/grasping/grasp_util.cpp
@@ -4,14 +4,6 @@
#include "grasp_util.h"
#include <tf2/transform_datatypes.h>
-// Actionlib
-#include <actionlib/client/simple_action_client.h>
-#include <actionlib/client/simple_client_goal_state.h>
-
-// Franka
-// if this should be required, the dependency to franka_gripper must also be declared
-#include <franka_gripper/MoveAction.h>
-
void GraspUtil::setupOpenGripper(trajectory_msgs::JointTrajectory &posture, double amount) {
posture.joint_names.resize(2);
posture.joint_names[0] = "panda_finger_joint1";
@@ -164,27 +156,3 @@ GraspUtil::placeFromAbove(moveit::planning_interface::MoveGroupInterface &move_g
ROS_ERROR_STREAM("Error while executing place task. MoveItErrorCode: " << miec.val);
return false;
}
-
-void GraspUtil::resetGripperForNextAction() {
-
- actionlib::SimpleActionClient<franka_gripper::MoveAction> ac("franka_gripper/move", true);
-
- ROS_INFO("Waiting for reset action server to start.");
- ac.waitForServer();
-
- ROS_INFO("Reset action server started, sending goal.");
- franka_gripper::MoveGoal goal;
- goal.speed = 0.08;
- goal.width = 0.0;
- ac.sendGoal(goal);
-
- //wait for the action to return
- bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
-
- if (finished_before_timeout) {
- actionlib::SimpleClientGoalState state = ac.getState();
- ROS_INFO("Reset action finished: %s", state.toString().c_str());
- } else {
- ROS_INFO("Reset action did not finish before the time out.");
- }
-}
diff --git a/src/grasping/grasp_util.h b/src/grasping/grasp_util.h
index effadabd3ae6186285b0c10c33222ec562fd786c..a6545b0a31a89e72ec0ac13dac459d760221e42a 100644
--- a/src/grasping/grasp_util.h
+++ b/src/grasping/grasp_util.h
@@ -90,10 +90,6 @@ public:
bool placeFromAbove(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);
- /**
- * resets the real robot, to allow correct future movements and grasps
- */
- void resetGripperForNextAction();
};
#endif //PANDA_GRASPING_GRASP_UTIL_H