Update 'forward_kinematics_1dof_node' code

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py

@@ -62,9 +62,9 @@ class Forward_Kinematics_Node_1Dof(Node):
         
         self.publish_pose(intermediate_vector, quaternion)
         
-        self.construct_path(intermediate_vector)
+        self.construct_path(intermediate_vector, quaternion)
 
-    def construct_path(self, vector):
+    def construct_path(self, vector, quaternion):
         # RViz
         pose = PoseStamped()
         pose.header.frame_id = "map"
@@ -72,7 +72,7 @@ class Forward_Kinematics_Node_1Dof(Node):
         pose.pose.position.x = vector[0]
         pose.pose.position.y = vector[1]
         pose.pose.position.z = vector[2]
-        pose.pose.orientation.w = 1.0
+        pose.pose.orientation.w = quaternion[3]
 
         # if the path is completed clear it
         if self.phi == 0: