Create topic publishing data to 'plotjuggler'

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()

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