Fix RViz visualization bug in 'forward_kinematics_2dof_2' node

ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py

@@ -76,7 +76,7 @@ class Forward_Kinematics_Node_2Dof(Node):
         self.construct_path(rotated_intermediate_vector)
 
     def translation(self):
-        # definte the desired height
+        # define the desired height
         h = 1
         # split h
         h_fraction = self.phi * h
@@ -95,18 +95,18 @@ class Forward_Kinematics_Node_2Dof(Node):
         # construct the path of the translation
         self.construct_path(translated_vector)
 
-    def construct_path(self, vector_path):
+    def construct_path(self, vector):
         # RViz
         pose = PoseStamped()
         pose.header.frame_id = "map"
         pose.header.stamp = self.get_clock().now().to_msg()
-        pose.pose.position.x = vector_path[0]
-        pose.pose.position.y = vector_path[1]
-        pose.pose.position.z = vector_path[2]
+        pose.pose.position.x = vector[0]
+        pose.pose.position.y = vector[1]
+        pose.pose.position.z = vector[2]
         pose.pose.orientation.w = 1.0
 
         #If the path is completed clear it
-        if self.phi == 0:
+        if (self.phi == 0 and self.index % 2 == 0):
             self.path.poses.clear()
         else:
             self.path.poses.append(pose)