From 56bafa5e3ea84f146e136ef831c5f9ce90e3a3d0 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Thu, 3 Dec 2020 19:41:34 +0100
Subject: [PATCH] fix missing dependency to franka_gripper, move
franka-specific things into new library
---
CMakeLists.txt | 25 +++++++++++---------
package.xml | 3 ++-
src/grasping/franka_gripper_util.cpp | 35 ++++++++++++++++++++++++++++
src/grasping/franka_gripper_util.h | 19 +++++++++++++++
src/grasping/grasp_util.cpp | 32 -------------------------
src/grasping/grasp_util.h | 4 ----
6 files changed, 70 insertions(+), 48 deletions(-)
create mode 100644 src/grasping/franka_gripper_util.cpp
create mode 100644 src/grasping/franka_gripper_util.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index d6d5ca2..065815a 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 ef381ee..94db6d0 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 0000000..08a458f
--- /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 0000000..a583763
--- /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 d305862..64f79e2 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 effadab..a6545b0 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
--
GitLab