diff --git a/launch/urdf_rviz.launch.py b/launch/urdf_rviz.launch.py new file mode 100644 index 0000000000000000000000000000000000000000..33eb557bc9e091136b303c048bacef22fc0ec6d4 --- /dev/null +++ b/launch/urdf_rviz.launch.py @@ -0,0 +1,20 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[{ + 'robot_description': open( + '/home/admim/ros2_ws/install/ur_description/share/ur_description/urdf/ur.urdf.xacro', 'r').read() + }] + ), + Node( + package='rviz2', + executable='rviz2', + output='screen' + ) + ]) \ No newline at end of file diff --git a/my_robot_package/joint_reader.py b/my_robot_package/joint_reader.py new file mode 100755 index 0000000000000000000000000000000000000000..1648e38d8deb2a5d82e09ec77cb99a9c9b18dcf7 --- /dev/null +++ b/my_robot_package/joint_reader.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +import rclpy +import math +from rclpy.node import Node +from sensor_msgs.msg import JointState +from rtde_receive import RTDEReceiveInterface + +class JointReaderNode(Node): + def __init__(self): + super().__init__('joint_reader') + + # Replace with the IP of your UR simulator VM + self.rtde_r = RTDEReceiveInterface("192.168.178.144") + + self.publisher = self.create_publisher(JointState, 'joint_states', 10) + self.timer = self.create_timer(0.1, self.publish_joint_states) # 10 Hz + + self.get_logger().info("Connected to UR RTDE. Publishing joint states.") + + def publish_joint_states(self): + try: + # UR joint names (standard 6 DOF) + joint_names = [ + "shoulder_pan_joint", # base + "shoulder_lift_joint", # shoulder + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint" + ] + joint_positions = None + joint_positions = self.rtde_r.getActualQ() # List of 6 joint angles + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.name = joint_names + msg.position = joint_positions + self.publisher.publish(msg) + + joint_degrees = [round(math.degrees(p), 2) for p in joint_positions] + log = ", ".join(f"{n}: {d}°" for n, d in zip(joint_names, joint_degrees)) + self.get_logger().info(f"Joint angles (deg): {log}") + + except Exception as e: + self.get_logger().warn(f"Failed to read joint data: {e}") + +def main(args=None): + rclpy.init(args=args) + node = JointReaderNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/setup.py b/setup.py index a5f7542a5f69ca78f51ca6129763c548dff44de9..3183c997d2658e23cf53b4dab1d80d18694c9b26 100644 --- a/setup.py +++ b/setup.py @@ -1,4 +1,6 @@ from setuptools import find_packages, setup +from glob import glob +import os package_name = 'my_robot_package' @@ -10,6 +12,7 @@ setup( ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, @@ -20,6 +23,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'joint_reader = my_robot_package.joint_reader:main', ], }, )