Scribd
Upload
23 views9 pages

Robotics

Uploaded by

ar22chb0a41
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
23 views9 pages

Robotics

Uploaded by

ar22chb0a41
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
You are on page 1/ 9

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

You might also like