feat(panda_control_server): imporve set_gripper_width action

This commit simplifies the logic inside the `set_gripper_width` action.
Previously we used both the `franka_gripper/grasp` and
`franka_gripper/move` services but after
frankaemika/franka_ros#209 has been merged we
can simply use the `franka_gripper/gripper_action` service.

panda_gazebo/src/panda_gazebo/core/control_server.py

@@ -26,7 +26,7 @@
 import numpy as np
 import rospy
 from controller_manager_msgs.srv import ListControllers, ListControllersRequest
-from franka_gripper.msg import GraspAction, GraspGoal, MoveAction, MoveGoal
+from control_msgs.msg import GripperCommandAction, GripperCommandGoal
 from franka_msgs.msg import FrankaState
 from panda_gazebo.common import ActionClientState
 from panda_gazebo.common.functions import (
@@ -256,65 +256,35 @@ def __init__(  # noqa: C901
             sys.exit(0)
 
         # Connect to the gripper command action server
-        # IMPROVE: We currently need to use both the 'franka_gripper/grasp' and
-        # 'franka_gripper/move' actions since the 'franka_gripper/gripper_command'.
-        # We can replace these services by the 'franka_gripper/gripper_action' service
-        # if https://github.com/frankaemika/franka_ros/issues/172 is solved.
-        franka_gripper_move_topic = "franka_gripper/move"
-        rospy.logdebug("Connecting to '%s' action service." % franka_gripper_move_topic)
-        if action_server_exists(franka_gripper_move_topic):
-            # Connect to robot control action server
-            self._gripper_move_client = actionlib.SimpleActionClient(
-                franka_gripper_move_topic,
-                MoveAction,
-            )
-
-            # Waits until the action server has started up
-            retval = self._gripper_move_client.wait_for_server(
-                timeout=rospy.Duration(secs=5)
-            )
-            if not retval:
-                rospy.logwarn(
-                    "No connection could be established with the '%s' service. "
-                    "The Panda Robot Environment therefore can not use this action "
-                    "service to control the Panda Robot." % (franka_gripper_move_topic)
-                )
-            else:
-                self._gripper_move_client_connected = True
-        else:
-            rospy.logwarn(
-                "No connection could be established with the '%s' service. "
-                "The Panda Robot Environment therefore can not use this action "
-                "service to control the Panda Robot." % (franka_gripper_move_topic)
-            )
-        franka_gripper_grasp_topic = "franka_gripper/grasp"
+        franka_gripper_command_topic = "franka_gripper/gripper_command"
         rospy.logdebug(
-            "Connecting to '%s' action service." % franka_gripper_grasp_topic
+            "Connecting to '%s' action service." % franka_gripper_command_topic
         )
-        if action_server_exists(franka_gripper_grasp_topic):
+        if action_server_exists(franka_gripper_command_topic):
             # Connect to robot control action server
-            self._gripper_grasp_client = actionlib.SimpleActionClient(
-                franka_gripper_grasp_topic,
-                GraspAction,
+            self._gripper_command_client = actionlib.SimpleActionClient(
+                franka_gripper_command_topic,
+                GripperCommandAction,
             )
 
             # Waits until the action server has started up
-            retval = self._gripper_grasp_client.wait_for_server(
+            retval = self._gripper_command_client.wait_for_server(
                 timeout=rospy.Duration(secs=5)
             )
             if not retval:
                 rospy.logwarn(
                     "No connection could be established with the '%s' service. "
                     "The Panda Robot Environment therefore can not use this action "
-                    "service to control the Panda Robot." % (franka_gripper_grasp_topic)
+                    "service to control the Panda Robot."
+                    % (franka_gripper_command_topic)
                 )
             else:
-                self._gripper_grasp_client_connected = True
+                self._gripper_command_client_connected = True
         else:
             rospy.logwarn(
                 "No connection could be established with the '%s' service. "
                 "The Panda Robot Environment therefore can not use this action "
-                "service to control the Panda Robot." % (franka_gripper_grasp_topic)
+                "service to control the Panda Robot." % (franka_gripper_command_topic)
             )
 
         # Connect to franka_state message
@@ -1934,11 +1904,6 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
             :obj:`panda_gazebo.srv.SetGripperWidthResponse`: Service response.
         """
         resp = SetGripperWidthResponse()
-        franka_gripper_service_available = (
-            self._gripper_grasp_client_connected
-            if set_gripper_width_req.grasping
-            else self._gripper_move_client_connected
-        )
 
         # Check if gripper width is within boundaries
         if set_gripper_width_req.width < 0.0 or set_gripper_width_req.width > 0.08:
@@ -1949,32 +1914,19 @@ def _set_gripper_width_cb(self, set_gripper_width_req):
         else:
             gripper_width = set_gripper_width_req.width
 
-        # Create grasp/move message
-        if set_gripper_width_req.grasping:
-            req = GraspGoal()
-            req.width = gripper_width
-            req.epsilon.inner = GRASP_EPSILON
-            req.epsilon.outer = GRASP_EPSILON
-            req.speed = GRASP_SPEED
-            req.force = GRASP_FORCE if set_gripper_width_req.grasping else 0
-        else:
-            req = MoveGoal()
-            req.width = gripper_width
-            req.speed = GRASP_SPEED
+        # Create gripper command message
+        req = GripperCommandGoal()
+        req.position = gripper_width
+        req.max_effort = GRASP_FORCE if set_gripper_width_req.grasping else 0
 
         # Invoke 'franka_gripper' action service
-        if franka_gripper_service_available:
-            franka_gripper_service = (
-                self._gripper_grasp_client
-                if set_gripper_width_req.grasping
-                else self._gripper_move_client
-            )
+        if self._gripper_command_client_connected:
             self.gripper_width_setpoint = gripper_width
-            franka_gripper_service.send_goal(req)
+            self._gripper_command_client.send_goal(req)
 
             # Wait for result
             if set_gripper_width_req.wait:
-                gripper_result = franka_gripper_service.wait_for_result(
+                gripper_result = self._gripper_command_client.wait_for_result(
                     timeout=rospy.Duration.from_sec(ACTION_TIMEOUT)
                 )
                 resp.success = gripper_result