Create a publisher for the pose in '2dof' node

ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py

@@ -23,6 +23,8 @@ class Forward_Kinematics_Node_2Dof(Node):
         # create a publisher for the plotjuggler data
         self.angle_publisher_ = self.create_publisher(Float64, '/angle_2dof/plotjuggler', 10)
         self.height_publisher_ = self.create_publisher(Float64, '/height_2dof/plotjuggler', 10)
+        # create a publisher for the pose
+        self.pose_publisher_ = self.create_publisher(PoseStamped, '/end_effector_pose_2dof', 10)
         #RViz
         self.path = Path()
         self.path.header.frame_id = "map"
@@ -63,6 +65,8 @@ class Forward_Kinematics_Node_2Dof(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):
@@ -117,6 +121,19 @@ class Forward_Kinematics_Node_2Dof(Node):
         height_msg.data = height
         self.height_publisher_.publish(height_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)