From ac8d2263775dbf72f41d600a3d51fd2fc92a6479 Mon Sep 17 00:00:00 2001
From: uquzq <ventsi@DESKTOP-J41TDPJ>
Date: Tue, 25 Jun 2024 12:30:57 +0200
Subject: [PATCH] Create topic publishing data to 'plotjuggler'
---
.../forward_kinematics_node_1dof.py | 18 +++++++++++++++---
.../simple_robot_1dof/reference_node_1dof.py | 2 +-
2 files changed, 16 insertions(+), 4 deletions(-)
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
index c42395a..e877184 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
@@ -20,6 +20,8 @@ class Forward_Kinematics_Node_1Dof(Node):
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
+ # create a publisher for the plotjuggler angles
+ self.angle_publisher_ = self.create_publisher(Float64, '/angles_1dof/plotjuggler', 10)
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -33,10 +35,12 @@ class Forward_Kinematics_Node_1Dof(Node):
def transformation(self):
# Define the desired rotation here
- theta = -pi / 2
+ theta = - pi / 2
# Split theta
theta_fraction = self.phi * theta
+ self.publish_angle(theta_fraction)
+
# Define the vector
start_vector = np.array([-1.0, 0.0, 0.0, 1.0])
@@ -94,8 +98,16 @@ class Forward_Kinematics_Node_1Dof(Node):
# publish the marker
self.point_publisher_.publish(marker)
-def main(args=None):
- rclpy.init(args=args)
+
+ def publish_angle(self, angle):
+ angle_msg = Float64()
+ angle_msg.data = angle * 180 / pi
+ # angle_radians = angle * 180 / pi
+ # self.get_logger().info('Angle: %f' %angle_radians)
+ self.angle_publisher_.publish(angle_msg)
+
+def main(args = None):
+ rclpy.init(args = args)
node = Forward_Kinematics_Node_1Dof()
rclpy.spin(node)
node.destroy_node()
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
index 85b7f2d..714a5df 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
@@ -11,7 +11,7 @@ class Reference_Node_1DoF(Node):
def __init__(self):
super().__init__('reference_node_1dof')
#Create a publisher sending the vector as a message
- self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)#
+ self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
timer_period = 0.05 # seconds
self.timer = self.create_timer(timer_period, self.ref_point_publisher)
self.phi_start = 0.0
--
GitLab