From 75e00b3111e5807c2ac49f4f363a0692a2b90d20 Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Mon, 19 Aug 2024 15:30:30 +0900 Subject: [PATCH] general bug fix --- CMakeLists.txt | 10 +- sas_robot_driver_franka/__init__.py | 106 ++++++++++-------- src/hand/qros_effector_driver_franka_hand.cpp | 3 +- src/robot_dynamics/qros_robot_dynamics_py.cpp | 2 +- src/sas_robot_driver_franka_node.cpp | 28 +++-- 5 files changed, 78 insertions(+), 71 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 40d521b..952dead 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -244,22 +244,22 @@ install(TARGETS ##### PYBIND11 LIBRARY ROBOT_DYNAMICS ##### ament_python_install_package(${PROJECT_NAME}) -pybind11_add_module(_robot_dynamics 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(_robot_dynamics +target_include_directories(_qros_franka_robot_dynamics_py PUBLIC $ $) -target_compile_definitions(_robot_dynamics 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(_robot_dynamics PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics) +target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics) -install(TARGETS _robot_dynamics +install(TARGETS _qros_franka_robot_dynamics_py DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) ##END## PYBIND11 LIBRARY ROBOT_DYNAMICS ##### diff --git a/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py index 24bea56..91e5636 100644 --- a/sas_robot_driver_franka/__init__.py +++ b/sas_robot_driver_franka/__init__.py @@ -1,11 +1,14 @@ """ """ -from sas_robot_driver_franka._robot_dynamics import RobotDynamicsInterface, RobotDynamicsProvider +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 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, Move_Request, Move_Response, Grasp, Grasp_Request, Grasp_Response +from sas_robot_driver_franka_interfaces.srv import Move, Grasp from sas_robot_driver_franka_interfaces.msg import GripperState import os import threading @@ -31,21 +34,25 @@ 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) + self._status_callback, 10) - self.result_queue = Queue() + self.service_future: Union[rclpy.Future, None] = None # gripper state self.state_width = None self.state_max_width = None self.state_temperature = None self.state_is_grasped = None + self.spin_handler = self._default_spin_handler - def _is_service_ready(self, service): + def _default_spin_handler(self): + rclpy.spin_once(self.node) + + def _is_service_ready(self, service: Client): try: - self.node.get_logger().info("Waiting for service: " + service.service_name) - service.wait_for_service(timeout_sec=0.1) - return True + # self.node.get_logger().info("Waiting for service: " + service.service_name) + ret = service.wait_for_service(timeout_sec=0.1) + return ret except Exception as e: self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e)) return False @@ -74,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): """ @@ -88,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 """ @@ -124,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 """ @@ -132,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 @@ -142,11 +151,19 @@ class FrankaGripperInterface: Get the result of the last action :return: """ - if not self.result_queue.empty(): - response = self.result_queue.get() - self._moving = False - self._grasping = False - return response.success + if self.service_future is not None: + if self.service_future.done(): + response = self.service_future.result() + if isinstance(response, Move.Response): + 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") @@ -157,40 +174,33 @@ class FrankaGripperInterface: """ if not self.is_enabled(): raise Exception("Gripper is not enabled") - if not self._moving and not self._grasping: - raise Exception("Gripper is not moving or grasping") - if not self._moving or self._grasping: - raise Exception("Gripper is not moving or grasping") - while self._moving or self._grasping: - rclcpp_spin_some(self.node) - time.sleep(0.1) - 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 + if not self.is_busy(): + return + while not self.is_done(): + self.spin_handler() + time.sleep(0.01) def _move(self, width, speed): self._moving = True - request = Move_Request() - request.width = width - request.speed = speed - response = self.move_service.call(request) - self.result_queue.put(response) + # 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) def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer): self._grasping = True - request = Grasp_Request() - request.width = width - request.force = force - request.speed = speed - request.epsilon_inner = epsilon_inner - request.epsilon_outer = epsilon_outer - response = self.grasp_service.call(request) - self.result_queue.put(response) + # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed)) + request = Grasp.Request() + request.width = float(width) + request.force = float(force) + request.speed = float(speed) + request.epsilon_inner = float(epsilon_inner) + request.epsilon_outer = float(epsilon_outer) + # 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 diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index b04e624..2600bef 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -78,8 +78,7 @@ 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(), diff --git a/src/robot_dynamics/qros_robot_dynamics_py.cpp b/src/robot_dynamics/qros_robot_dynamics_py.cpp index 391c114..145adcd 100644 --- a/src/robot_dynamics/qros_robot_dynamics_py.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_py.cpp @@ -40,7 +40,7 @@ using RDC = qros::RobotDynamicsClient; using RDS = qros::RobotDynamicsServer; -PYBIND11_MODULE(_qros_robot_dynamic, m) +PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m) { py::class_(m, "RobotDynamicsClient") .def(py::init&,const std::string&>()) diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index eb13751..a871c50 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/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>("upper_torque_threshold");}catch (...){} - if(node->has_parameter("upper_torque_threshold")) { - RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set."); + try { std::vector 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(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>("upper_force_threshold");}catch (...){} - if(node->has_parameter("upper_force_threshold")) { + try { + std::vector 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 upper_torque_threshold_std_vec; - sas::get_ros_parameter(node,"upper_force_threshold",upper_torque_threshold_std_vec); - franka_interface_configuration.upper_force_threshold = std_vec_to_std_array(upper_torque_threshold_std_vec); - }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 = std_vec_to_std_array(upper_force_threshold_std_vec); + }catch(...) { + RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque 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("robot_parameter_file_path");}catch (...){} - if(node->has_parameter("robot_parameter_file_path")) - { + try { 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(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;