diff --git a/scripts/example_gripper_control.py b/scripts/example_gripper_control.py index f0f917a..c1a5008 100644 --- a/scripts/example_gripper_control.py +++ b/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) diff --git a/scripts/publish_dumy_robot_dynmaics.py b/scripts/publish_dumy_robot_dynmaics.py index c1e1102..0c27cf7 100644 --- a/scripts/publish_dumy_robot_dynmaics.py +++ b/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 + 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 diff --git a/scripts/subscribe_dummy_dynamic.py b/scripts/subscribe_dummy_dynamic.py index 136bba4..352bb34 100644 --- a/scripts/subscribe_dummy_dynamic.py +++ b/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") + 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") - while not dynam_provider.is_enabled(): - rospy.loginfo("Waiting for robot dynamics provider to be enabled") - rospy.sleep(1) - - rospy.loginfo("Subscribing to dummy robot dynamics") - rate = rospy.Rate(50) - - while not rospy.is_shutdown(): - force = dynam_provider.get_stiffness_force() - torque = dynam_provider.get_stiffness_torque() - ee_pose = dynam_provider.get_stiffness_frame_pose() - 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() \ No newline at end of file + 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) + 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()