attempt to merge newer changes

CMakeLists.txt

@@ -244,22 +244,22 @@ install(TARGETS
 ##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
 ament_python_install_package(${PROJECT_NAME})
 
-pybind11_add_module(_qros_franka_robot_dynamic SHARED
+pybind11_add_module(_qros_franka_robot_dynamics_py SHARED
         src/robot_dynamics/qros_robot_dynamics_py.cpp
         src/robot_dynamics/qros_robot_dynamics_client.cpp
         src/robot_dynamics/qros_robot_dynamics_server.cpp
 )
 
-target_include_directories(_qros_franka_robot_dynamic
+target_include_directories(_qros_franka_robot_dynamics_py
         PUBLIC
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
         $<INSTALL_INTERFACE:include/robot_dynamics>)
 
-target_compile_definitions(_qros_franka_robot_dynamic PRIVATE IS_SAS_PYTHON_BUILD)
+target_compile_definitions(_qros_franka_robot_dynamics_py PRIVATE IS_SAS_PYTHON_BUILD)
 # https://github.com/pybind/pybind11/issues/387
-target_link_libraries(_qros_franka_robot_dynamic PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
+target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
 
-install(TARGETS _qros_franka_robot_dynamic
+install(TARGETS _qros_franka_robot_dynamics_py
         DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
 )
 ##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####

sas_robot_driver_franka/__init__.py

@@ -1,11 +1,14 @@
 """
 """
-from sas_robot_driver_franka._qros_franka_robot_dynamic import RobotDynamicsClient, RobotDynamicsServer
+from typing import Union
+
+from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
 
 import rclpy
 from rclpy.node import Node
-# from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from rclpy.client import Client
-from sas_robot_driver_franka_interfaces.srv import Move, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka_interfaces.srv import Move, Grasp
 from sas_robot_driver_franka_interfaces.msg import GripperState
 import os
 import threading
@@ -31,9 +34,9 @@ class FrankaGripperInterface:
         self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
         self._grasping = False
         self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
-                                                          self._status_callback, 5)
+                                                          self._status_callback, 10)
 
-        self.result_queue = Queue()
+        self.service_future: Union[rclpy.Future, None] = None
 
         # gripper state
         self.state_width = None
@@ -45,14 +48,11 @@ class FrankaGripperInterface:
     def _default_spin_handler(self):
         rclpy.spin_once(self.node)
 
-    def set_spin_handler(self, spin_handler):
+    def _is_service_ready(self, service: Client):
-        self.spin_handler = spin_handler
-
-    def _is_service_ready(self, service):
         try:
-            self.node.get_logger().info("Waiting for service: " + service.service_name)
+            # self.node.get_logger().info("Waiting for service: " + service.service_name)
-            service.wait_for_service(timeout_sec=0.1)
+            ret = service.wait_for_service(timeout_sec=0.1)
-            return True
+            return ret
         except Exception as e:
             self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
             return False
@@ -81,7 +81,7 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        threading.Thread(target=self._move, args=(width, speed)).start()
+        self._move(width, speed)
 
     def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
         """
@@ -95,7 +95,7 @@ class FrankaGripperInterface:
         """
         if self.is_busy():
             raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
-        threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
+        self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
 
     def get_max_width(self):
         """ Get the maximum width of the gripper """
@@ -131,7 +131,7 @@ class FrankaGripperInterface:
 
     def is_busy(self):
         """ Check if the gripper is currently moving or grasping """
-        return self._moving or self._grasping
+        return self._moving or self._grasping or self.service_future is not None
 
     def is_done(self):
         """ Check if the gripper is done moving or grasping """
@@ -139,7 +139,9 @@ class FrankaGripperInterface:
             self.node.get_logger().warn("Gripper is not moving or grasping")
             return False
         else:
-            if self.result_queue.empty():
+            if self.service_future is not None:
+                if self.service_future.done():
+                    return True
                 return False
             else:
                 return True
@@ -149,11 +151,19 @@ class FrankaGripperInterface:
         Get the result of the last action
         :return:
         """
-        if not self.result_queue.empty():
+        if self.service_future is not None:
-            response = self.result_queue.get()
+            if self.service_future.done():
-            self._moving = False
+                response = self.service_future.result()
-            self._grasping = False
+                if isinstance(response, Move.Response):
-            return response.success
+                    self._moving = False
+                elif isinstance(response, Grasp.Response):
+                    self._grasping = False
+                else:
+                    raise Exception("Invalid response type")
+                self.service_future = None
+                return response.success
+            else:
+                raise Exception("No result available")
         else:
             raise Exception("No result available")
 
@@ -164,40 +174,33 @@ class FrankaGripperInterface:
         """
         if not self.is_enabled():
             raise Exception("Gripper is not enabled")
-        if not self._moving and not self._grasping:
+        if not self.is_busy():
-            raise Exception("Gripper is not moving or grasping")
+            return
-        if not self._moving or self._grasping:
+        while not self.is_done():
-            raise Exception("Gripper is not moving or grasping")
-        while self._moving or self._grasping:
             self.spin_handler()
-            time.sleep(0.1)
+            time.sleep(0.01)
-            if not self.result_queue.empty():
-                response = self.result_queue.get()
-                if isinstance(response, Move_Response):
-                    self._moving = False
-                elif isinstance(response, Grasp_Response):
-                    self._grasping = False
-                else:
-                    raise Exception("Invalid response type")
-                break
-
-        return response.success
 
     def _move(self, width, speed):
         self._moving = True
-        request = Move_Request()
+        # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
-        request.width = width
+        request = Move.Request()
-        request.speed = speed
+        request.width = float(width)
-        response = self.move_service.call(request)
+        request.speed = float(speed)
-        self.result_queue.put(response)
+        # self.node.get_logger().info("Calling move service")
+        self.service_future = self.move_service.call_async(request)
 
     def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
         self._grasping = True
-        request = Grasp_Request()
+        # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
-        request.width = width
+        request = Grasp.Request()
-        request.force = force
+        request.width = float(width)
-        request.speed = speed
+        request.force = float(force)
-        request.epsilon_inner = epsilon_inner
+        request.speed = float(speed)
-        request.epsilon_outer = epsilon_outer
+        request.epsilon_inner = float(epsilon_inner)
-        response = self.grasp_service.call(request)
+        request.epsilon_outer = float(epsilon_outer)
-        self.result_queue.put(response)
+        # self.node.get_logger().info("Calling grasp service")
+        self.service_future = self.grasp_service.call_async(request)
+
+
+    def set_spin_handler(self, spin_handler):
+        self.spin_handler = spin_handler

src/hand/qros_effector_driver_franka_hand.cpp

@@ -78,12 +78,11 @@ namespace qros
     }
 
 
-    void EffectorDriverFrankaHand::start_control_loop()
+    void EffectorDriverFrankaHand::start_control_loop() {
-    {
         sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
         clock.init();
-        RCLCPP_INFO_STREAM(node_->get_logger(),
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
-            "["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
         while (!(*break_loops_))
         {
             if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
@@ -141,7 +140,7 @@ namespace qros
         {
             throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
         }
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
     }
 
     void EffectorDriverFrankaHand::initialize()
@@ -151,6 +150,14 @@ namespace qros
         gripper_homing();
         // start gripper status loop
         status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
+        // check status loop with timeout
+        auto time_now = std::chrono::system_clock::now();
+        auto time_out = time_now + std::chrono::seconds(5);
+        while (!status_loop_running_)
+        {
+            if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
     }
 
     void EffectorDriverFrankaHand::deinitialize()
@@ -243,13 +250,12 @@ namespace qros
 
     void EffectorDriverFrankaHand::_gripper_status_loop()
     {
-        status_loop_running_ = true;
         sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
-        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
+        RCLCPP_INFO_STREAM(node_->get_logger(),"["+ std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
-        ;
         clock.init();
         try
         {
+            status_loop_running_ = true;
             while (status_loop_running_)
             {
 #ifdef BLOCK_READ_IN_USED

src/robot_dynamics/qros_robot_dynamics_py.cpp

@@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient;
 using RDS = qros::RobotDynamicsServer;
 
 
-PYBIND11_MODULE(_qros_franka_robot_dynamic, m)
+PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
 {
     py::class_<RDC>(m, "RobotDynamicsClient")
         .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())

src/sas_robot_driver_franka_node.cpp

@@ -118,35 +118,33 @@ int main(int argc, char **argv)
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
     }
 
-    try{node->declare_parameter<std::vector<double>>("upper_torque_threshold");}catch (...){}
+    try {
-    if(node->has_parameter("upper_torque_threshold")) {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
         std::vector<double> upper_torque_threshold_std_vec;
         sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
         franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
-    }else {
+    }catch(...) {
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
     }
-    try{node->declare_parameter<std::vector<double>>("upper_force_threshold");}catch (...){}
+    try {
-    if(node->has_parameter("upper_force_threshold")) {
+        std::vector<double> upper_force_threshold_std_vec;
+        sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
-        std::vector<double> upper_torque_threshold_std_vec;
+        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
-        sas::get_ros_parameter(node,"upper_force_threshold",upper_torque_threshold_std_vec);
+    }catch(...) {
-        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
-    }else {
-        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold not set. Using default with value scalling.");
         franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
     }
-    try{node->declare_parameter<std::string>("robot_parameter_file_path");}catch (...){}
+    try {
-    if(node->has_parameter("robot_parameter_file_path"))
-    {
         std::string robot_parameter_file_path;
         sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
         RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
         const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
         robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
-    }else{RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");}
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");
+    }
 
     robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;