When I run this code for turtlesim the turtle just spins in place forever and never moves linearly to the intermediate points. ROS Noetic on Ubuntu 20.04

edit

#!/usr/bin/env python

import rospy import math from turtlesim.msg import Pose from geometry_msgs.msg import Twist, Point

x = 0 y = 0 theta = 0

def pose_callback(pose): global x, y, theta x = pose.x y = pose.y theta = pose.theta

def move_turtle_to_goal(goal): global x, y, theta velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10) pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, pose_callback) rate = rospy.Rate(10)

while not rospy.is_shutdown():
    # Calculate distance and angle to goal
    distance = math.sqrt(pow((goal.x - x), 2) + pow((goal.y - y), 2))
    angle = math.atan2(goal.y - y, goal.x - x)

    # Move turtle to goal
    if distance > 0.1:
        move_cmd = Twist()
        move_cmd.linear.x = 0.0
        move_cmd.angular.z = 4.0 * (angle - theta)
        velocity_publisher.publish(move_cmd)
        rospy.sleep(1)  # pause for 1 second to allow the turtle to rotate
        move_cmd.linear.x = min(0.5, 0.5 * distance)  # set linear velocity after rotation
        move_cmd.angular.z = 0.0  # stop rotation
        velocity_publisher.publish(move_cmd)
    else:
        break

    rate.sleep()

# Stop turtle when goal reached
move_cmd = Twist()
move_cmd.linear.x = 0.0
move_cmd.angular.z = 0.0
velocity_publisher.publish(move_cmd)

# Unsubscribe from the pose topic to prevent interference with the next intermediate point
pose_subscriber.unregister()

def move_turtle_to_intermediate_points(intermediate_points): for point in intermediate_points: print("Moving to intermediate point ({}, {})".format(point.x, point.y)) move_turtle_to_goal(point)

def get_points_from_user(): intermediate_points = [] num_points = int(input("Enter the number of intermediate points: ")) for i in range(num_points): x = float(input("Enter x coordinate of intermediate point {}: ".format(i+1))) y = float(input("Enter y coordinate of intermediate point {}: ".format(i+1))) intermediate_points.append(Point(x, y, 0.0)) return intermediate_points

if __name__ == '__main__': try: rospy.init_node('move_turtle_to_goal', anonymous=True) intermediate_points = get_points_from_user() end_point = Point(float(input("Enter x coordinate of end point: ")), float(input("Enter y coordinate of end point: ")), 0.0) move_turtle_to_intermediate_points(intermediate_points + [end_point]) except rospy.ROSInterruptException: pass