[message interface] combining driver and interface package for better future management

README.md

@@ -1,3 +0,0 @@
-![](https://img.shields.io/github/license/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka/master)![](https://img.shields.io/badge/status-experimental-red)
-# sas_robot_driver_franka
-

sas_robot_driver_franka/README.md

@@ -0,0 +1,84 @@
+![](https://img.shields.io/github/license/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka/ros2_jazzy)![](https://img.shields.io/badge/status-experimental-red)
+# sas_robot_driver_franka
+![ezgif com-video-to-gif (1)](https://github.com/SmartArmStack/sas_robot_driver/assets/23158313/b4e1efa7-8d93-4a67-ab87-a74c41d8f4bc)
+Robot driver for the Franka Research 3 Robot. 
+
+## Original Contributors
+- [Juan José Quiroz Omaña](https://github.com/juanjqo/sas_robot_driver_franka)
+
+## dependencies
+SAS: 
+- sas_common
+- sas_core
+- sas_msgs
+- sas_conversions
+- sas_robot_driver
+- [sas_robot_driver_franka_interfaces](https://www.github.com/qlin960618/sas_robot_driver_franka_interfaces)
+
+Misc: 
+- geometry_msgs
+- std_msgs
+- std_srvs
+- tf2_ros
+- tf2
+
+Franka Related
+
+- Franka (libfranka)
+- pinocchio
+
+## Components
+
+### sas_robot_driver_franka
+Kernel-space real-time robot driver for the SmartArm compatible control
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.004  # control sampling time (1.0/loop_rate)
+"q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895]  # joint limit minimum, if not define will obtain from [ROBOT_JSON]
+"q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895]  # joint limit maximum, if not define will obtain from [ROBOT_JSON]
+"force_upper_limits_scaling_factor": 4.0  # force and torque error limit scaling factor from default, (if any of the below is not defined)
+"upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # joint torque safety threshold, [j1, j2, ..., j7]
+"upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # end-effector frame safety force threshold [x, y, z, rx, ry, rz] 
+"robot_mode": "VelocityControl"  # control mode [VelocityControl/PositionControl]: currently PositionControl can be unstable 
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```
+
+### sas_robot_driver_franka_hand
+Franka Hand driver for basic control of gripping functionality.
+
+#### special dependencies
+- sas_robot_driver_franka_interfaces
+  - <sas_robot_driver_franka_interfaces/srv/grasp.hpp>
+  - <sas_robot_driver_franka_interfaces/srv/move.hpp>
+  - <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.02  # control sampling time, (1.0/status update rate)
+"default_force": 2.0  # default gripping force (overridable from service call)
+"default_speed": 0.07  # default gripping speed (overridable from service call)
+"default_epsilon_inner": 0.007  # default grip call position epsilon (overridable from service call)
+"default_epsilon_outer": 0.007  # default grip call position epsilon (overridable from service call)
+```
+
+
+### sas_robot_driver_coppelia_node
+Digital twin mirroring node for driver node to CoppeliaSim
+
+
+#### node parameters:
+```yaml
+"thread_sampling_time_sec": 0.008  # control sampling time (1.0/loop_rate)
+"vrep_ip": os.environ["VREP_IP"]  # ip of the vrep computer
+"vrep_port": 20012  # vrep port
+"vrep_dynamically_enabled": True  # if vrep scene is dynamically enabled
+"using_real_robot": True  # if node should read-only from real robot driver, False for simulation mode
+"vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"]  # coppelia scene joint names
+"robot_topic_prefix": "/arm3"  # robot driver namespace
+"robot_mode": "PositionControl"  # only use when in simulation node, aka using_real_robot==False, for simulating velocity control mode. (TODO: currently VelocityControl is not stable)
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```

sas_robot_driver_franka/package.xml

@@ -2,9 +2,9 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>sas_robot_driver_franka</name>
-  <version>0.0.0</version>
+  <version>0.0.15</version>
   <description>The sas_driver_franka package</description>
-  <maintainer email="moonshot@todo.todo">moonshot</maintainer>
+  <maintainer email="qlin1806@g.ecc.u-tokyo.ac.jp">qlin</maintainer>
 
   <license>LGPLv3</license>
   <buildtool_depend>ament_cmake</buildtool_depend>
@@ -17,7 +17,7 @@
   <depend>sas_core</depend>
   <depend>sas_robot_driver</depend>
   <depend>sas_common</depend>
-  <depend>sas_robot_driver_franka_interface</depend>
+  <depend>sas_robot_driver_franka_interfaces</depend>
 
 
   <test_depend>ament_lint_auto</test_depend>

sas_robot_driver_franka/sas_robot_driver_franka/__init__.py

@@ -7,15 +7,10 @@ from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamics
 import rclpy
 from rclpy.node import Node
 from rclpy.client import Client
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
 from sas_robot_driver_franka_interfaces.srv import Move, Grasp
 from std_srvs.srv import Trigger
 from sas_robot_driver_franka_interfaces.msg import GripperState
 import os
-import threading
-from queue import Queue
-import time
-import struct
 
 MOVE_TOPIC_SUFFIX = "move"
 GRASP_TOPIC_SUFFIX = "grasp"
@@ -27,16 +22,14 @@ class FrankaGripperInterface:
     Non blocking interface to control the Franka gripper
     """
 
-    def __init__(self, node: Node, robot_prefix):
+    def __init__(self, node: Node, robot_prefix: str):
         self.last_message = None
         self.robot_prefix = robot_prefix
         self.node = node
+        self._current_action = None  # between moving, grasping, and homing
         self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
-        self._moving = False
         self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
-        self._grasping = False
         self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
-        self._homing = False
         self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
                                                           self._status_callback, 10)
 
@@ -50,9 +43,9 @@ class FrankaGripperInterface:
         self.spin_handler = self._default_spin_handler
 
     def _default_spin_handler(self):
-        rclpy.spin_once(self.node)
+        rclpy.spin_once(self.node, timeout_sec=0.01)
 
-    def _is_service_ready(self, service: Client):
+    def _is_service_ready(self, service: Client) -> bool:
         try:
             # self.node.get_logger().info("Waiting for service: " + service.service_name)
             ret = service.wait_for_service(timeout_sec=0.1)
@@ -61,7 +54,7 @@ class FrankaGripperInterface:
             self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
             return False
 
-    def is_enabled(self):
+    def is_enabled(self) -> bool:
         if self.state_width is None:
             return False
         if not self._is_service_ready(self.move_service):
@@ -75,7 +68,11 @@ class FrankaGripperInterface:
         self.state_max_width = msg.max_width
         self.state_temperature = msg.temperature
         self.state_is_grasped = msg.is_grasped
-    def home(self):
+
+    #############################################################################
+    # Public methods to control the gripper (actions)
+    #############################################################################
+    def home(self) -> rclpy.Future:
         """
         Reinitialize the gripper
         :return: None
@@ -84,7 +81,7 @@ class FrankaGripperInterface:
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
         self._home()
 
-    def move(self, width, speed=0):
+    def move(self, width, speed=0) -> rclpy.Future:
         """
         Move the gripper to a specific width
         :param width: width in meters
@@ -93,9 +90,9 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._move(width, speed)
+        return self._move(width, speed)
 
-    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
+    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0) -> rclpy.Future:
         """
         Grasp an object with the gripper
         :param width:
@@ -107,49 +104,59 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
+        return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
 
-    def get_max_width(self):
+    #############################################################################
+    # Public methods to get the gripper state, from ros topics
+    #############################################################################
+    def get_max_width(self) -> float:
         """ Get the maximum width of the gripper """
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
         return self.state_max_width
 
-    def get_width(self):
+    def get_width(self) -> float:
         """ Get the current width of the gripper """
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
         return self.state_width
 
-    def get_temperature(self):
+    def get_temperature(self) -> float:
         """ Get the temperature of the gripper """
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
         return self.state_temperature
 
-    def is_grasped(self):
+    def is_grasped(self) -> bool:
         """ Check if an object is grasped """
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
         return self.state_is_grasped
 
+    #############################################################################
+    # relating to action state
+    #############################################################################
     def is_moving(self):
         """ Check if the gripper is currently moving """
-        return self._moving
+        return self._current_action == "moving"
 
     def is_grasping(self):
         """ Check if the gripper is currently grasping """
-        return self._grasping
+        return self._current_action == "grasping"
+
+    def is_homing(self):
+        """ Check if the gripper is currently homing """
+        return self._current_action == "homing"
 
     def is_busy(self):
         """ Check if the gripper is currently moving or grasping """
-        return self._moving or self._grasping or self.service_future is not None
+        return self._current_action is not None
 
     def is_done(self):
         """ Check if the gripper is done moving or grasping """
         if not self.is_busy():
             self.node.get_logger().warn("Gripper is not moving or grasping")
-            return False
+            return True
         else:
             if self.service_future is not None:
                 if self.service_future.done():
@@ -158,6 +165,9 @@ class FrankaGripperInterface:
             else:
                 return True
 
+    ##############################################################################
+    # Public methods to get the result of the last action
+    ##############################################################################
     def get_result(self):
         """
         Get the result of the last action
@@ -167,11 +177,11 @@ class FrankaGripperInterface:
             if self.service_future.done():
                 response = self.service_future.result()
                 if isinstance(response, Move.Response):
-                    self._moving = False
+                    self._current_action = None
                 elif isinstance(response, Grasp.Response):
-                    self._grasping = False
+                    self._current_action = None
                 elif isinstance(response, Trigger.Response):
-                    self._homing = False
+                    self._current_action = None
                 else:
                     raise Exception("Invalid response type")
                 self.service_future = None
@@ -181,6 +191,7 @@ class FrankaGripperInterface:
                 raise Exception("No result available")
         else:
             raise Exception("No result available")
+
     def get_last_message(self):
         return self.last_message
 
@@ -189,31 +200,36 @@ class FrankaGripperInterface:
         Wait until the gripper is done moving or grasping
         :return: success
         """
+        rate = self.node.create_rate(100)  # 100 Hz
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
         if not self.is_busy():
             return
         while not self.is_done():
             self.spin_handler()
-            time.sleep(0.01)
+            rate.sleep()
-    def _home(self):
+        return self.get_result()
-        self._homing = True
+
+    def _home(self) -> rclpy.Future:
+        self._current_action = "homing"
         # self.node.get_logger().info("Homing gripper")
         request = Trigger.Request()
         # self.node.get_logger().info("Calling home service")
         self.service_future = self.home_service.call_async(request)
+        return self.service_future
 
-    def _move(self, width, speed):
+    def _move(self, width, speed) -> rclpy.Future:
-        self._moving = True
+        self._current_action = "moving"
         # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
         request = Move.Request()
         request.width = float(width)
         request.speed = float(speed)
         # self.node.get_logger().info("Calling move service")
         self.service_future = self.move_service.call_async(request)
+        return self.service_future
 
-    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
+    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer) -> rclpy.Future:
-        self._grasping = True
+        self._current_action = "grasping"
         # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
         request = Grasp.Request()
         request.width = float(width)
@@ -223,7 +239,7 @@ class FrankaGripperInterface:
         request.epsilon_outer = float(epsilon_outer)
         # self.node.get_logger().info("Calling grasp service")
         self.service_future = self.grasp_service.call_async(request)
-
+        return self.service_future
 
     def set_spin_handler(self, spin_handler):
         self.spin_handler = spin_handler

sas_robot_driver_franka/src/hand/qros_effector_driver_franka_hand.cpp

@@ -122,7 +122,7 @@ namespace qros
         return;
 #endif
         RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
+        gripper_sptr_.reset();
         gripper_sptr_ = nullptr;
     }
 

sas_robot_driver_franka_interfaces/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(sas_robot_driver_franka_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/GripperState.msg"
+        "srv/Move.srv"
+        "srv/Grasp.srv"
+)
+
+install(
+        DIRECTORY msg
+        DESTINATION share/${PROJECT_NAME}/msg
+)
+
+install(
+        DIRECTORY srv
+        DESTINATION share/${PROJECT_NAME}/srv
+)
+
+
+
+
+ament_package()

sas_robot_driver_franka_interfaces/msg/GripperState.msg

@@ -0,0 +1,5 @@
+float32 width
+float32 max_width
+bool is_grasped
+uint16 temperature
+uint64 duration_ms

sas_robot_driver_franka_interfaces/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sas_robot_driver_franka_interfaces</name>
+  <version>0.0.0</version>
+  <description>sas_driver_franka interfaces package</description>
+  <maintainer email="qlin1806@gmail.com">qlin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sas_robot_driver_franka_interfaces/srv/Grasp.srv

@@ -0,0 +1,7 @@
+float32 width
+float32 speed
+float32 force
+float32 epsilon_inner
+float32 epsilon_outer
+---
+bool success

sas_robot_driver_franka_interfaces/srv/Move.srv

@@ -0,0 +1,4 @@
+float32 width
+float32 speed
+---
+bool success