diff --git a/.DS_Store b/.DS_Store index 9fd8c92..3844f5c 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/workspace/.DS_Store b/workspace/.DS_Store index 46e8f7f..ddfa767 100644 Binary files a/workspace/.DS_Store and b/workspace/.DS_Store differ diff --git a/workspace/src/.DS_Store b/workspace/src/.DS_Store index ddb12ef..a3f334b 100644 Binary files a/workspace/src/.DS_Store and b/workspace/src/.DS_Store differ diff --git a/workspace/src/6_DOF_example.pd b/workspace/src/6_DOF_example.pd new file mode 100644 index 0000000..7f88683 --- /dev/null +++ b/workspace/src/6_DOF_example.pd @@ -0,0 +1,52 @@ +#N canvas 827 239 527 327 12; +#X obj 942 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X msg 1226 778 disconnect; +#X obj 989 837 netsend -u -b; +#X obj 897 787 osc.format; +#X text 954 457 x; +#X text 1001 457 y; +#X text 1046 457 z; +#X text 918 585 0; +#X text 918 484 +1; +#X text 918 686 -1; +#X obj 989 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X obj 1034 487 vsl 43 215 -1 1 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X obj 1090 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X obj 1134 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X obj 1178 487 vsl 43 215 -3.141 3.141 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 1; +#X msg 1189 754 connect localhost 8000; +#X text 1099 457 roll; +#X text 1134 457 pitch; +#X text 1189 457 yaw; +#X msg 708 718 /speed_scaling; +#X obj 1034 768 pack s f f f f f f; +#X obj 815 487 vsl 43 215 0.1 0.9 0 0 empty empty empty 0 -9 0 10 #e4e4e4 #4d4d4d #373737 0 0; +#X text 815 457 speed; +#X text 1222 484 +pi; +#X text 1222 580 0; +#X text 1226 686 -pi; +#X text 776 484 90%; +#X text 776 686 10%; +#X obj 764 768 pack s f; +#X msg 856 741 /tcp_coordinates; +#X connect 0 0 29 0; +#X connect 0 0 20 1; +#X connect 1 0 2 0; +#X connect 3 0 2 0; +#X connect 10 0 29 0; +#X connect 10 0 20 2; +#X connect 11 0 29 0; +#X connect 11 0 20 3; +#X connect 12 0 29 0; +#X connect 12 0 20 4; +#X connect 13 0 29 0; +#X connect 13 0 20 5; +#X connect 14 0 29 0; +#X connect 14 0 20 6; +#X connect 15 0 2 0; +#X connect 19 0 28 0; +#X connect 20 0 3 0; +#X connect 21 0 28 1; +#X connect 21 0 19 0; +#X connect 28 0 3 0; +#X connect 29 0 20 0; diff --git a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc index 12715e7..1164a9c 100644 Binary files a/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc and b/workspace/src/osc_ros2/osc_ros2/__pycache__/osc_ros2.cpython-310.pyc differ diff --git a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py index 2c89247..e3c6712 100644 --- a/workspace/src/osc_ros2/osc_ros2/osc_ros2.py +++ b/workspace/src/osc_ros2/osc_ros2/osc_ros2.py @@ -27,7 +27,6 @@ class JointNameListener(Node): self.joint_names = None def joint_state_callback(self, msg: JointState): - print("Joint names received from JointState message:") self.joint_names = list(msg.name) class OSC_ROS2_interface(Node): @@ -36,27 +35,6 @@ class OSC_ROS2_interface(Node): def __init__(self, joint_names, joint_velocity_limits, robot, cost_mask): super().__init__('scaled_joint_trajectory_publisher') - while True: - try: - self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() - if self.trajectory_topic_name == "": - self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' - break - elif self.trajectory_topic_name.startswith("/"): - break - else: - print("Invalid topic name. A valid topic name should start with '/'.") - except Exception as e: - print(f"An error occurred: {e}") - - - - # ROS2 Publisher - self.publisher = self.create_publisher( - JointTrajectory, - self.trajectory_topic_name, - 1 - ) self.subscription = self.create_subscription( JointState, @@ -89,52 +67,8 @@ class OSC_ROS2_interface(Node): } self.speed_scaling = 0.2 self.new = False + self.n_joints = len(joint_names) - - while True: - try: - print('+-' * 50) - log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if log_ip == "": - log_ip = "127.0.0.1" - print('--' * 50) - log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") - if log_port == "": - log_port = 5005 - else: - log_port = int(log_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) - if state_ip == "": - state_ip = "127.0.0.1" - print('--' * 50) - state_port = input("Enter the target port for the log messages (or press Enter for default: 7000): ") - if state_port == "": - state_port = 7000 - else: - state_port = int(state_port) - break - except ValueError: - print("Invalid input. Please enter a valid IP address.") - continue - while True: - try: - print('+-' * 50) - commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") - if commands_port == "": - commands_port = 8000 - else: - commands_port = int(commands_port) - break - except ValueError: - print("Invalid input. Please enter a valid port.") - continue if robot: while True: print('+-' * 50) @@ -240,39 +174,105 @@ class OSC_ROS2_interface(Node): try: ''' else: - if not(self.joint_names): - - while True: - print('-+'*50) - print("Joint names:") - print(self.joint_names) - print('-'*50) - correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: (y/n)?: ").strip() - if correct.lower() == 'y': - break - elif correct.lower() == 'n': + while True: + print('+-' * 50) + update_limits = input("Do you want to set joint limits? (y/n): ").strip().lower() + if update_limits == 'y': + self.joint_lim = [[None]*self.n_joints,[None]*self.n_joints] + for i, joint in enumerate(self.joint_names): while True: - joint_names = [] - print('-+'*50) - print("Enter the joint names manually one by one. Type 'done' when you are finished:") - print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") - while True: - print('-'*50) - joint_name = input("Enter joint name (or 'done' to finish): ").strip() - if joint_name.lower() == 'done': - break - if joint_name: - joint_names.append(joint_name) - print('-+'*50) - correct = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() - if correct.lower() == 'y': + try: + print("-" * 50) + lower_limit = input(f"Enter the new lower limit for joint '{joint}' (or press Enter to keep current): ").strip() + upper_limit = input(f"Enter the new upper limit for joint '{joint}' (or press Enter to keep current): ").strip() + + if lower_limit and upper_limit and float(lower_limit) >= float(upper_limit): + print('--' * 50) + print(" !!! Invalid input. Lower limit must be less than upper limit. !!! ") + continue + self.joint_lim[0][i] = float(lower_limit) if lower_limit!="" else None + self.joint_lim[1][i] = float(upper_limit) if upper_limit!="" else None break - else: - print("Please re-enter the joint names.") - print('invalid input. Please enter "y" or "n".') + except ValueError: + print("Invalid input. Please enter numeric values or leave blank to keep current limits.") + break + elif update_limits == 'n': + self.joint_lim = None + print("Invalid input. Please enter 'y' or 'n'.") + + print(f'New limits for joint:\n lower: {self.joint_lim[0]}\n upper: {self.joint_lim[1]}') + + + while True: + try: + print('+-' * 50) + self.trajectory_topic_name = input("Enter the topic name to which the joint trajectory should be sent (press Enter for default: '/scaled_joint_trajectory_controller/joint_trajectory'): ").strip() + if self.trajectory_topic_name == "": + self.trajectory_topic_name = '/scaled_joint_trajectory_controller/joint_trajectory' + break + elif self.trajectory_topic_name.startswith("/"): + break + else: + print("Invalid topic name. A valid topic name should start with '/'.") + except Exception as e: + print(f"An error occurred: {e}") + + # ROS2 Publisher + self.publisher = self.create_publisher( + JointTrajectory, + self.trajectory_topic_name, + 1 + ) + + while True: + try: + print('+-' * 50) + log_ip = str(input("Enter the target IP on which you want to recieve the log as OSC Messages (or press Enter for default: '127.0.0.1'): ")) + if log_ip == "": + log_ip = "127.0.0.1" + print('--' * 50) + log_port = input("Enter the target port for the log messages (or press Enter for default: 5005): ") + if log_port == "": + log_port = 5005 + else: + log_port = int(log_port) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + state_ip = str(input("Enter the target IP on which you want to recieve the joint states as OSC Messages (or press Enter for default: '127.0.0.1'): ")) + if state_ip == "": + state_ip = "127.0.0.1" + print('--' * 50) + state_port = input("Enter the target port for the joint state messages (or press Enter for default: 7000): ") + if state_port == "": + state_port = 7000 + else: + state_port = int(state_port) + break + except ValueError: + print("Invalid input. Please enter a valid IP address.") + continue + while True: + try: + print('+-' * 50) + commands_port = input("Enter the port you want to send your commands to (or press Enter for default: 8000): ") + if commands_port == state_port: + print("The commands port must be different from the state port.") + continue + if commands_port == "": + commands_port = 8000 + else: + commands_port = int(commands_port) + break + except ValueError: + print("Invalid input. Please enter a valid port.") + continue + - self.n_joints = len(joint_names) - osc_startup() osc_udp_client(state_ip, state_port, "osc_client") @@ -289,7 +289,18 @@ class OSC_ROS2_interface(Node): osc_method("/cartesian_trajectory", self.cartesian_trajectory_handler, argscheme=osm.OSCARG_DATAUNPACK) osc_method("/speed_scaling", self.speed_scaling_handler, argscheme=osm.OSCARG_DATAUNPACK) print('--' * 50) - self.hz = float(input("Enter the desired refresh frequency (Hz): ")) + while True: + try: + print('+-' * 50) + self.hz = input("Enter the desired refresh frequency (Hz) (or press Enter for default: 100): ") + if self.hz == "": + self.hz = 100 + else: + self.hz = float(self.hz) + break + except ValueError: + print("Invalid input. Please enter a valid number.") + continue print() print('=-=' * 50) print() @@ -306,6 +317,7 @@ class OSC_ROS2_interface(Node): print('=-=' * 50) print() + self.get_logger().info(f"Publishing joint trajectory to {self.trajectory_topic_name}") self.get_logger().info(f'Ready to receive OSC messages on {socket.gethostbyname(socket.gethostname())}:{commands_port}') self.get_logger().info(f'Sending joint states to {state_ip}:{state_port}') self.get_logger().info(f'Sending log messages to {log_ip}:{log_port}') @@ -854,16 +866,53 @@ def main(): rclpy.spin_once(node) joint_names = node.joint_names node.destroy_node() - ''' if joint_names: + correct_names = input("The following joint names were found:\n" + "\n".join(joint_names) + "\nAre these correct? (y/n): ").strip().lower() while True: - try: - joint_velocity_limits = {name: float(input(f"Enter the velocity limit for joint '{name}' (or press Enter to skip): ").strip())} for name in joint_names} + if correct_names == 'y': break - except ValueError: - print("Invalid input. Please enter numeric values or leave blank to skip.") - ''' - joint_velocity_limits = None + elif correct_names == 'n': + joint_names = None + break + correct_names = input("Invalid input. Please enter 'y' or 'n'.") + if not(joint_names): + print("Please enter the joint names manually.") + while True: + joint_names = [] + print('-+'*50) + print("Enter the joint names manually one by one. Type 'done' when you are finished:") + print("Attention: The order of the joints is important. It should be the same in which you want to send the joint positions.") + while True: + print('-'*50) + joint_name = input("Enter joint name (or 'done' to finish): ").strip() + if joint_name.lower() == 'done': + break + if joint_name: + joint_names.append(joint_name) + print('-+'*50) + correct_names = input(f"Are those the joint names as defined in your JointTrajectroy recieving Node?: {joint_names}. (y/n)?: ").strip() + if correct_names.lower() == 'y': + break + else: + print("Please re-enter the joint names.") + while True: + try: + joint_velocity_limits = {} + for name in joint_names: + while True: + try: + print('--'*50) + limit = input(f"Enter the velocity limit for joint '{name}': ").strip() + if limit == "": + continue + else: + joint_velocity_limits[name] = float(limit) + break + except ValueError: + print("Invalid input. Please enter a numeric value or press Enter to skip.") + break + except ValueError: + print("Invalid input. Please enter numeric values or leave blank to skip.") robot = None cost_mask = None break