Create a publisher for the pose of the end effector

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py

@@ -22,6 +22,8 @@ class Forward_Kinematics_Node_1Dof(Node):
         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)
+        # create a publisher for the pose
+        self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose', 10)
         #RViz
         self.path = Path()
         self.path.header.frame_id = "map"
@@ -35,14 +37,14 @@ 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])
+        start_vector = np.array([1.0, 0.0, 0.0, 1.0])
 
         # Define the matrix
         intermediate_rotation = np.array([
@@ -54,7 +56,9 @@ class Forward_Kinematics_Node_1Dof(Node):
         
         # Calculate the intermediate vectors
         intermediate_vector = np.matmul(intermediate_rotation, start_vector)
-
+        
+        self.publish_pose(intermediate_vector)
+        
         self.construct_path(intermediate_vector)
 
     def construct_path(self, vector):
@@ -104,6 +108,21 @@ class Forward_Kinematics_Node_1Dof(Node):
         angle_msg.data = angle * 180 / pi
         self.angle_publisher_.publish(angle_msg)
 
+
+    def publish_pose(self, vector):
+        pose_msg = PoseStamped()
+        pose_msg.header.frame_id = "map"
+        pose_msg.header.stamp = self.get_clock().now().to_msg()
+        pose_msg.pose.position.x = vector[0]
+        pose_msg.pose.position.y = vector[1]
+        pose_msg.pose.position.z = vector[2]
+        pose_msg.pose.orientation.x = 0.0
+        pose_msg.pose.orientation.y = 0.0
+        pose_msg.pose.orientation.z = 0.0
+        pose_msg.pose.orientation.w = 1.0
+        self.pose_publisher_.publish(pose_msg)
+
+         
 def main(args = None):
     rclpy.init(args = args)
     node = Forward_Kinematics_Node_1Dof()