Merge branch 'master' into feature/tests

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)

package.xml

 <?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>

src/grasping/franka_gripper_util.cpp

+//
+// 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.");
+    }
+}

src/grasping/franka_gripper_util.h

+//
+// 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

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.");
-    }
-}

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