From 48022a9fdc682733e3f926e186de69a42c7b611c Mon Sep 17 00:00:00 2001
From: oliverkania <oliver.kania@rwth-aachen>
Date: Thu, 17 Apr 2025 13:12:40 +0000
Subject: [PATCH] added joint reader and rviz
---
launch/urdf_rviz.launch.py | 20 +++++++++++
my_robot_package/joint_reader.py | 58 ++++++++++++++++++++++++++++++++
setup.py | 4 +++
3 files changed, 82 insertions(+)
create mode 100644 launch/urdf_rviz.launch.py
create mode 100755 my_robot_package/joint_reader.py
diff --git a/launch/urdf_rviz.launch.py b/launch/urdf_rviz.launch.py
new file mode 100644
index 0000000..33eb557
--- /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 0000000..1648e38
--- /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 a5f7542..3183c99 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',
],
},
)
--
GitLab