Merge remote-tracking branch 'gitea/ros2_jazzy' into ros2_jazzy

include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp

@@ -69,6 +69,7 @@ namespace qros {
 struct EffectorDriverFrankaHandConfiguration
 {
     std::string robot_ip;
+    bool initialize_with_homing = true;
     double thread_sampeling_time_s = 1e8;  // 10Hz
     double default_force = 3.0;
     double default_speed = 0.1;

sas_robot_driver_franka/__init__.py

@@ -9,6 +9,7 @@ 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
@@ -27,12 +28,15 @@ class FrankaGripperInterface:
     """
 
     def __init__(self, node: Node, robot_prefix):
+        self.last_message = None
         self.robot_prefix = robot_prefix
         self.node = node
         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)
 
@@ -71,6 +75,14 @@ class FrankaGripperInterface:
         self.state_max_width = msg.max_width
         self.state_temperature = msg.temperature
         self.state_is_grasped = msg.is_grasped
+    def home(self):
+        """
+        Reinitialize the gripper
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        self._home()
 
     def move(self, width, speed=0):
         """
@@ -158,14 +170,19 @@ class FrankaGripperInterface:
                     self._moving = False
                 elif isinstance(response, Grasp.Response):
                     self._grasping = False
+                elif isinstance(response, Trigger.Response):
+                    self._homing = False
                 else:
                     raise Exception("Invalid response type")
                 self.service_future = None
+                self.last_message = hasattr(response, "message") and response.message or ""
                 return response.success
             else:
                 raise Exception("No result available")
         else:
             raise Exception("No result available")
+    def get_last_message(self):
+        return self.last_message
 
     def wait_until_done(self):
         """
@@ -179,6 +196,12 @@ class FrankaGripperInterface:
         while not self.is_done():
             self.spin_handler()
             time.sleep(0.01)
+    def _home(self):
+        self._homing = True
+        # 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)
 
     def _move(self, width, speed):
         self._moving = True

scripts/example_gripper_control.py

@@ -1,6 +1,5 @@
 import threading
 
-from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
 import rclpy
 from sas_robot_driver_franka import FrankaGripperInterface
 import time
@@ -14,13 +13,11 @@ def main_loop(node):
 
     while not hand1.is_enabled():
         logger.info("Waiting for gripper to be enabled...")
-        rclcpp_spin_some(node)
         time.sleep(0.1)
     rclpy.node.get_logger(node_name).info("Gripper enabled!")
 
     def spin_all(node_):
         while rclpy.ok():
-            rclcpp_spin_some(node_)
             rclpy.spin_once(node_, timeout_sec=0.1)
             time.sleep(0.1)
             

scripts/publish_dumy_robot_dynmaics.py

@@ -1,29 +1,46 @@
-import rospy
+import time
-from sas_robot_driver_franka import RobotDynamicsServer
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
 import dqrobotics as dql
 import numpy as np
 
 
-if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_provider")
 
-    dynam_provider = RobotDynamicsProvider("/franka1")
+
+if __name__ == "__main__":
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_server_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_server")
+    dynam_provider = RobotDynamicsServer(sas_node, "/franka1")
+
     t = dql.DQ([0, 0, 1])
     r = dql.DQ([1, 0, 0, 0])
     base_pose = r + 0.5 * dql.E_ * t * r
     dynam_provider.set_world_to_base_tf(base_pose)
     t_ = 0
-    rospy.loginfo("Publishing dummy robot dynamics")
+    node.get_logger().info("Dummy robot dynamics server started")
     r = dql.DQ([0, 0, 0, 1])
-    rate = rospy.Rate(100)
+    rate = node.create_rate(100)
     dummy_force = np.random.rand(3) * 100
     dummy_torque = np.random.rand(3) * 10
+    try:
+        while rclpy.ok():
+            t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
+            ee_pose = r + 0.5 * dql.E_ * t * r
+            dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
+            dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
+            dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
+            rclcpp_spin_some(sas_node)
+            rclpy.spin_once(node)
+            t_ += 0.01
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
 
-    while not rospy.is_shutdown():
-        t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
-        ee_pose = r + 0.5 * dql.E_ * t * r
-        dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
-        dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
-        dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
-        rate.sleep()
-        t_ += 0.01

scripts/subscribe_dummy_dynamic.py

@@ -1,30 +1,53 @@
-import rospy
+import time
-from sas_robot_driver_franka import RobotDynamicsInterface
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
 import dqrobotics as dql
 import numpy as np
 
 from dqrobotics.interfaces.vrep import DQ_VrepInterface
 
-vrep = DQ_VrepInterface()
+# vrep = DQ_VrepInterface()
-vrep.connect("192.168.10.103", 19997, 100, 10)
+# vrep.connect("192.168.10.103", 19997, 100, 10)
+vrep = None
 
 if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_subscriber")
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_client_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_client")
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin_once(timeout_sec=0.1)
+        dynam_provider = RobotDynamicsClient(sas_node, "/franka1")
+        rclcpp_spin_some(sas_node)
+        while not dynam_provider.is_enabled():
+            node.get_logger().info("Waiting for robot dynamics provider to be enabled")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            node.get_logger().info("retrying...")
+            time.sleep(0.5)
 
-    dynam_provider = RobotDynamicsInterface("/franka1")
+        node.get_logger().info("Dummy robot dynamics client started")
-    while not dynam_provider.is_enabled():
+        while rclpy.ok():
-        rospy.loginfo("Waiting for robot dynamics provider to be enabled")
+            force = dynam_provider.get_stiffness_force()
-        rospy.sleep(1)
+            torque = dynam_provider.get_stiffness_torque()
-
+            ee_pose = dynam_provider.get_stiffness_frame_pose()
-    rospy.loginfo("Subscribing to dummy robot dynamics")
+            if vrep is not None:
-    rate = rospy.Rate(50)
+                vrep.set_object_pose("xd1", ee_pose)
-
+            node.get_logger().info(f"EE Pose: {ee_pose}")
-    while not rospy.is_shutdown():
+            node.get_logger().info(f"Force: {force}")
-        force = dynam_provider.get_stiffness_force()
+            node.get_logger().info(f"Torque: {torque}")
-        torque = dynam_provider.get_stiffness_torque()
+            rclcpp_spin_some(sas_node)
-        ee_pose = dynam_provider.get_stiffness_frame_pose()
+            executor.spin_once(timeout_sec=0.1)
-        vrep.set_object_pose("xd1", ee_pose)
+            time.sleep(0.5)
-        rospy.loginfo("EE Pose: %s", ee_pose)
+    except KeyboardInterrupt:
-        rospy.loginfo("Force: %s", force)
+        pass
-        rospy.loginfo("Torque: %s", torque)
+    finally:
-        rate.sleep()
+        rclcpp_shutdown()
+        node.destroy_node()
+        if vrep is not None:
+            vrep.disconnect()
+        rclpy.shutdown()

src/hand/qros_effector_driver_franka_hand.cpp

@@ -149,7 +149,9 @@ namespace qros
     {
         if (!_is_connected()) throw std::runtime_error(
             "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
-        gripper_homing();
+        if (configuration_.initialize_with_homing) {
+            gripper_homing();
+        }
         // start gripper status loop
         status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
         // check status loop with timeout

src/sas_robot_driver_franka_hand_node.cpp

@@ -53,10 +53,10 @@ void sig_int_handler(int)
 template<typename T>
 void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
 {
-    if(node->has_parameter(param_name))
+    try
     {
         sas::get_ros_parameter(node,param_name,param);
-    }else
+    }catch (const std::exception& e)
     {
         RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
     }
@@ -81,6 +81,7 @@ int main(int argc, char **argv)
     RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
 
     qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+    get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing);
     sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
     sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s);
     sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);