Not following Plan about vizanti HOT 5 OPEN

#!/usr/bin/env python

import rospy
import actionlib
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal

class WaypointFollower:
    def __init__(self):
        rospy.init_node('waypoint_follower')

        # Subscribe to the /waypoints topic
        rospy.Subscriber('/waypoints', Path, self.waypoints_callback)

        # Create a SimpleActionClient for move_base
        self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)

        # Wait for the move_base action server to start
        rospy.loginfo("Waiting for move_base action server...")
        self.client.wait_for_server()
        rospy.loginfo("Connected to move_base action server")

        self.waypoints = []
        self.current_goal_index = 0

    def waypoints_callback(self, msg):
        rospy.loginfo("Received waypoints")
        self.waypoints = msg.poses
        self.current_goal_index = 0
        self.send_next_goal()

    def send_next_goal(self):
        if self.current_goal_index < len(self.waypoints):
            goal = MoveBaseGoal()
            goal.target_pose = self.waypoints[self.current_goal_index]
            goal.target_pose.header.stamp = rospy.Time.now()
            rospy.loginfo(f"Sending goal {self.current_goal_index + 1}/{len(self.waypoints)}: {goal.target_pose.pose}")
            self.client.send_goal(goal, done_cb=self.done_callback)
        else:
            rospy.loginfo("All waypoints reached")

    def done_callback(self, state, result):
        rospy.loginfo(f"Goal {self.current_goal_index + 1} reached")
        self.current_goal_index += 1
        self.send_next_goal()

if __name__ == '__main__':
    try:
        WaypointFollower()
        rospy.spin()
    except rospy.ROSInterruptException:
        rospy.loginfo("Waypoint follower node terminated.")

#!/usr/bin/env python

import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import Point
from slamware_ros_sdk.msg import MoveToLocationsRequest, MoveOptions

class PathToMoveToLocations:
    def __init__(self):
        rospy.init_node('path_to_move_to_locations')
        
        self.publisher = rospy.Publisher('/move_to_locations', MoveToLocationsRequest, queue_size=10)
        
        rospy.Subscriber('/path', Path, self.path_callback)
        
        rospy.loginfo("Path to MoveToLocations node initialized.")
        
    def path_callback(self, path_msg):
        move_request = MoveToLocationsRequest()
        
        # Convert the Path waypoints to Point[]
        move_request.locations = [pose.pose.position for pose in path_msg.poses]
        
        # Set default MoveOptions
        move_request.options = MoveOptions()
        
        # Set end yaw
        move_request.yaw = 0.0
        
        # Publish the MoveToLocationsRequest message
        self.publisher.publish(move_request)

if __name__ == '__main__':
    try:
        PathToMoveToLocations()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass