fix example script for RobotDynamics and gripper control

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
-from sas_robot_driver_franka import RobotDynamicsServer
+import time
+
+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
-
-    while not rospy.is_shutdown():
+    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)
-        rate.sleep()
+            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()
+

scripts/subscribe_dummy_dynamic.py

@@ -1,30 +1,53 @@
-import rospy
-from sas_robot_driver_franka import RobotDynamicsInterface
+import time
+
+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.connect("192.168.10.103", 19997, 100, 10)
+# vrep = DQ_VrepInterface()
+# vrep.connect("192.168.10.103", 19997, 100, 10)
+vrep = None
 
 if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_subscriber")
-
-    dynam_provider = RobotDynamicsInterface("/franka1")
+    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():
-        rospy.loginfo("Waiting for robot dynamics provider to be enabled")
-        rospy.sleep(1)
+            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)
 
-    rospy.loginfo("Subscribing to dummy robot dynamics")
-    rate = rospy.Rate(50)
-
-    while not rospy.is_shutdown():
+        node.get_logger().info("Dummy robot dynamics client started")
+        while rclpy.ok():
             force = dynam_provider.get_stiffness_force()
             torque = dynam_provider.get_stiffness_torque()
             ee_pose = dynam_provider.get_stiffness_frame_pose()
+            if vrep is not None:
                 vrep.set_object_pose("xd1", ee_pose)
-        rospy.loginfo("EE Pose: %s", ee_pose)
-        rospy.loginfo("Force: %s", force)
-        rospy.loginfo("Torque: %s", torque)
-        rate.sleep()
+            node.get_logger().info(f"EE Pose: {ee_pose}")
+            node.get_logger().info(f"Force: {force}")
+            node.get_logger().info(f"Torque: {torque}")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            time.sleep(0.5)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        if vrep is not None:
+            vrep.disconnect()
+        rclpy.shutdown()