Turtle_controlling without p controller

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
robot_x = 0
robot_y = 0
def pose_callback(pose):
global robot_x
global robot_y
rospy.loginfo('Robot X = %f\n',pose.x)
rospy.loginfo('Robot Y = %f\n',pose.y)
robot_x = pose.x
robot_y = pose.y

def move(lin_vel, lin_vel_y, ang_vel, dist_x,dist_y):

global robot_x
global robot_y
rospy.init_node('letter', anonymous=True)
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
sub = rospy.Subscriber('/turtle1/pose', Pose, pose_callback)
rospy.Rate(10)
vel = Twist()
while not rospy.is_shutdown():
vel.linear.x = lin_vel
vel.angular.z = ang_vel
vel.linear.y = lin_vel_y

if(ang_vel == 0):

if(round(robot_x,1) == dist_x and round(robot_y,1) ==

dist_y):
rospy.loginfo("destination reached " )
break
else:
pub.publish(vel)
else:
if(round(robot_x,1) > dist_x or round(robot_y,1) > dist_y):
rospy.loginfo("rotation reached")
break
else:
pub.publish(vel)

if __name__=='__main__':
 try:
move(0,0.5,0,5.5,6.0)
move(-0.5,0,0,5.0,6.0)
move(0,-0.5,0,5.0,5.6)
move(-0.5,0,-0.5,5.5,6.2)

except rospy.ROSInterruptException:
pass

Turtle_bot controller with p controller

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow, atan2, sqrt

class TurtleBot:

def __init__(self):
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher =
rospy.Publisher('/turtle1/cmd_vel',Twist, queue_size=10)

self.pose_subscriber = rospy.Subscriber('/turtle1/pose',Pose,
self.update_pose)

self.pose = Pose()
self.rate = rospy.Rate(10)

def update_pose(self, data):

self.pose = data
self.pose.x = round(self.pose.x, 4)
self.pose.y = round(self.pose.y, 4)
def euclidean_distance(self, goal_pose):
 return sqrt(pow((goal_pose.x - self.pose.x), 2) +
pow((goal_pose.y - self.pose.y), 2))

def linear_vel(self, goal_pose, constant=1.5):

return constant * self.euclidean_distance(goal_pose)

def steering_angle(self, goal_pose):

return atan2(goal_pose.y - self.pose.y, goal_pose.x - self.pose.x)

def angular_vel(self, goal_pose, constant=6):

return constant * (self.steering_angle(goal_pose) -

self.pose.theta)

def move2goal(self):

goal_pose = Pose()

goal_pose.x = float(input("Set your x goal: "))

goal_pose.y = float(input("Set your y goal: "))

distance_tolerance = input("Set your tolerance: ")

vel_msg = Twist()

while vel

vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0

vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)

self.velocity_publisher.publish(vel_msg)

self.rate.sleep()

vel_msg.linear.x = 0
vel_msg.angular.z = 0
 self.velocity_publisher.publish(vel_msg)

rospy.spin()

if __name__ == '__main__':
try:
x = TurtleBot()
x.move2goal()
except rospy.ROSInterruptException:
pass

turtlesim modified

#!/usr/bin/env python3

import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import sqrt , atan2,pow

robot_x = 0
robot_y = 0
def pose_callback(pose):
global robot_x
global robot_y
rospy.loginfo('Robot X = %d\n',pose.x)
rospy.loginfo('Robot Y = %d\n',pose.y)
robot_x = pose.x
robot_y = pose.y

def move( dist_x,dist_y):

global robot_x
global robot_y
rospy.init_node('letter', anonymous=True)
pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
sub = rospy.Subscriber('/turtle1/pose', Pose, pose_callback)
rospy.Rate(10)
vel = Twist()
constant = 1.5
linear_vel = constant * sqrt(pow((dist_x - robot_x), 2) +
pow((dist_y - robot_y), 2))
 ang_vel = constant * ( atan2(dist_y - robot_y, dist_x - robot_x) -
Pose.theta)

while not rospy.is_shutdown():

vel.linear.x = linear_vel
vel.angular.z = ang_vel

if(sqrt(pow((dist_x - robot_x), 2) +pow((dist_y - robot_y), 2))

>= 0.1):
pub.publish(vel)
else:
break
rospy.spin()

if __name__=='__main__':
try:
move(7,7)
#move(-0.5,0,0,5.0,6.0)
#move(0,-0.5,0,5.0,5.6)
#move(-0.5,0,-0.5,5.5,6.2)

except rospy.ROSInterruptException:
pass

ANOTHER SOURCE CODE

#!/usr/bin/env python3
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow, atan2, sqrt,pi

class TurtleBot:

def _init_(self):
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist, queue_size=10)
self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
 self.pose = Pose()
self.rate = rospy.Rate(10)

def update_pose(self, data):

self.pose = data
self.pose.x = round(self.pose.x, 4)
self.pose.y = round(self.pose.y, 4)

def euclidean_distance(self, goal_pose):

return sqrt(pow((goal_pose.x - self.pose.x), 2) +
pow((goal_pose.y - self.pose.y), 2))

def linear_vel(self, goal_pose, constant=1.5):

return constant * self.euclidean_distance(goal_pose)

def steering_angle(self, goal_pose):

return atan2(goal_pose.y - self.pose.y, goal_pose.x -
self.pose.x)

def angular_vel(self, goal_pose, constant=6):

return constant * (self.steering_angle(goal_pose) -
self.pose.theta)

def move2goal(self):

init_pose = Pose()
init_pose.x = 0.0
init_pose.y = 0.0
init_pose.theta=-0.5*pi

path_b = [(5.544445,5.544445),(7.544445, 5.544445), (8.544445,

6.544445), (8.544445, 8.544445), (7.544445, 9.544445), (5.544445,
9.544445)]

vel_msg = Twist()

for point in path_b:

goal_pose = Pose()
goal_pose.x = point[0]
goal_pose.y = point[1]

distance_tolerance = 0.1 # Set a tolerance for the

position error

while self.euclidean_distance(goal_pose) >=

distance_tolerance:
 # P-controller
vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0

vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)

self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)

rospy.spin()

if _name_ == '_main_':
try:
x = TurtleBot()
x.move2goal()
except rospy.ROSInterruptException:
pass

[11:52 pm, 20/08/2023] Arif 🐧: #!/usr/bin/env python3

import rclpy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
robot_x = 0
robot_y = 0
def pose_callback(pose):
rclpy.loginfo('Robot X = %f\n',pose.x)
rclpy.loginfo('Robot Y = %f\n',pose.y)
robot_x = pose.x
robot_y = pose.y

def move(lin_vel, lin_vel_y, ang_vel, dist_x,dist_y):

global robot_x
global robot_y
rclpy.init_node('letter', anonymous=True)
pub = rclpy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
sub = rclpy.Subscriber('/turtle1/pose', Pose, pose_callback)
rclpy.Rate(10)
vel = Twist()
 while not rclpy.is_shutdown():
vel.linear.x = lin_vel
vel.angular.z = ang_vel
vel.linear.y = lin_vel_y

if(ang_vel == 0):

[11:52 pm, 20/08/2023] Arif 🐧: #!/usr/bin/env python3

import rclpy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
from math import pow, atan2, sqrt,pi

class TurtleBot:

def init(self):
rclpy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rclpy.Publisher('/turtle1/cmd_vel',
Twist, queue_size=10)
self.pose_subscriber = rclpy.Subscriber('/turtle1/pose', Pose,
self.update_pose)
self.pose = Pose()
self.rate = rclpy.Rate(10)

def update_pose(self, data):

self.pose = data
self.pose.x = round(self.pose.x, 4)
self.pose.y = round(self.pose.y, 4)

def euclidean_distance(self, goal_pose):

return sqrt(pow((goal_pose.x - self.pose.x), 2) +
pow((goal_pose.y - self.pose.y), 2))

def linear_vel(self, goal_pose, constant=0.5):

return constant * self.euclidean_distance(goal_pose)

def steering_angle(self, goal_pose):

return atan2(goal_pose.y - self.pose.y, goal_pose.x -
self.pose.x)

def angular_vel(self, goal_pose, constant=6):

return constant * (self.steering_angle(goal_pose) -
self.pose.theta)

def move2goal(self):
 init_pose = Pose()
init_pose.x = 0.0
init_pose.y = 0.0
init_pose.theta=-0.5*pi

path = [(5.544445,5.544445),(5.544445, 6.544445), (4.544445,

6.544445), (4.544445, 5.544445), (3.544445, 6.544445), (4.544445,
6.544445)]

vel_msg = Twist()

for point in path:

goal_pose = Pose()
goal_pose.x = point[0]
goal_pose.y = point[1]

distance_tolerance = 0.1

while self.euclidean_distance(goal_pose) >=

distance_tolerance:

vel_msg.linear.x = self.linear_vel(goal_pose)
vel_msg.linear.y = 0
vel_msg.linear.z = 0

vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = self.angular_vel(goal_pose)

self.velocity_publisher.publish(vel_msg)
self.rate.sleep()
vel_msg.linear.x = 0
vel_msg.angular.z = 0
self.velocity_publisher.publish(vel_msg)

rclpy.spin()

if _name_ == 'main':
try:
x = TurtleBot()
x.move2goal()
except rclpy.ROSInterruptException:
pass