#!/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 moveturtletogoal(goal): global x, y, theta velocitypublisher = rospy.Publisher('/turtle1/cmdvel', Twist, queuesize=10) posesubscriber = rospy.Subscriber('/turtle1/pose', Pose, posecallback) 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 moveturtletointermediatepoints(intermediatepoints): for point in intermediatepoints: print("Moving to intermediate point ({}, {})".format(point.x, point.y)) moveturtleto_goal(point)

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

if name == 'main': try: rospy.initnode('moveturtletogoal', anonymous=True) intermediatepoints = getpointsfromuser() endpoint = Point(float(input("Enter x coordinate of end point: ")), float(input("Enter y coordinate of end point: ")), 0.0) moveturtletointermediatepoints(intermediatepoints + [end_point]) except rospy.ROSInterruptException: pass