Moving in a Straight Line

1 !/usr/bin/env python
2 import rospy
3 from geometry_msgs.msg import Twist
4
5 def move():
6 # Starts a new node
7 rospy.init_node('robot_cleaner', anonymous=True)
8 velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist,
queue_size=10)
9 vel_msg = Twist()
10
11 #Receiveing the user's input
12 print("Let's move your robot")
13 speed = input("Input your speed:")
14 distance = input("Type your distance:")
15 isForward = input("Foward?: ")#True or False
16
17 #Checking if the movement is forward or backwards
18 if(isForward):
19 vel_msg.linear.x = abs(speed)
20 else:
21 vel_msg.linear.x = -abs(speed)
22 #Since we are moving just in x-axis
23 vel_msg.linear.y = 0
24 vel_msg.linear.z = 0
25 vel_msg.angular.x = 0
26 vel_msg.angular.y = 0
27 vel_msg.angular.z = 0
28
29 while not rospy.is_shutdown():
30
31 #Setting the current time for distance calculus
32 t0 = rospy.Time.now().to_sec()
33 current_distance = 0
34
35 #Loop to move the turtle in an specified distance
36 while(current_distance < distance):
37 #Publish the velocity
38 velocity_publisher.publish(vel_msg)
39 #Takes actual time to velocity calculus
40 t1=rospy.Time.now().to_sec()
41 #Calculates distancePoseStamped
42 current_distance= speed*(t1-t0)
43 #After the loop, stops the robot
44 vel_msg.linear.x = 0
45 #Force the robot to stop
46 velocity_publisher.publish(vel_msg)
47
48 if __name__ == '__main__':
49 try:
50 #Testing our function
51 move()
52 except rospy.ROSInterruptException: pass
Rotating Left/Right
1 #!/usr/bin/env python
2 import rospy
3 from geometry_msgs.msg import Twist
4 PI = 3.1415926535897
5
6 def rotate():
7 #Starts a new node
8 rospy.init_node('robot_cleaner', anonymous=True)
9 velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist,
queue_size=10)
10 vel_msg = Twist()
11
12 # Receiveing the user's input
13 print("Let's rotate your robot")
14 speed = input("Input your speed (degrees/sec):")
15 angle = input("Type your distance (degrees):")
16 clockwise = input("Clockwise?: ") #True or false
17
18 #Converting from angles to radians
19 angular_speed = speed*2*PI/360
20 relative_angle = angle*2*PI/360
21
22 #We wont use linear components
23 vel_msg.linear.x=0
24 vel_msg.linear.y=0
25 vel_msg.linear.z=0
26 vel_msg.angular.x = 0
27 vel_msg.angular.y = 0
28
29 # Checking if our movement is CW or CCW
30 if clockwise:
31 vel_msg.angular.z = -abs(angular_speed)
32 else:
33 vel_msg.angular.z = abs(angular_speed)
34 # Setting the current time for distance calculus
35 t0 = rospy.Time.now().to_sec()
36 current_angle = 0
37
38 while(current_angle < relative_angle):
39 velocity_publisher.publish(vel_msg)
40 t1 = rospy.Time.now().to_sec()
41 current_angle = angular_speed*(t1-t0)
42
43
44 #Forcing our robot to stop
45 vel_msg.angular.z = 0
46 velocity_publisher.publish(vel_msg)
47 rospy.spin()
48
49 if __name__ == '__main__':
50 try:
51 # Testing our function
52 rotate()
53 except rospy.ROSInterruptException:
54 pass
Go to Goal
1 #!/usr/bin/env python
2 import rospy
3 from geometry_msgs.msg import Twist
4 from turtlesim.msg import Pose
5 from math import pow,atan2,sqrt
6
7 class turtlebot():
8
9 def __init__(self):
10 #Creating our node,publisher and subscriber
11 rospy.init_node('turtlebot_controller', anonymous=True)
12 self.velocity_publisher = rospy.Publisher('/turtle1/cmd_vel',
Twist, queue_size=10)
13 self.pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose,
self.callback)
14 self.pose = Pose()
15 self.rate = rospy.Rate(10)
16
17 #Callback function implementing the pose value received
18 def callback(self, data):
19 self.pose = data
20 self.pose.x = round(self.pose.x, 4)
21 self.pose.y = round(self.pose.y, 4)
22
23 def get_distance(self, goal_x, goal_y):
24 distance = sqrt(pow((goal_x - self.pose.x), 2) + pow((goal_y -
self.pose.y), 2))
25 return distance
26
27 def move2goal(self):
28 goal_pose = Pose()
29 goal_pose.x = input("Set your x goal:")
30 goal_pose.y = input("Set your y goal:")
31 distance_tolerance = input("Set your tolerance:")
32 vel_msg = Twist()
33
34 while sqrt(pow((goal_pose.x - self.pose.x), 2) + pow((goal_pose.y -
self.pose.y), 2)) >= distance_tolerance:
35
36 #Porportional Controller
37 #linear velocity in the x-axis:
38 vel_msg.linear.x = 1.5 * sqrt(pow((goal_pose.x - self.pose.x),
2) + pow((goal_pose.y - self.pose.y), 2))
39 vel_msg.linear.y = 0
40 vel_msg.linear.z = 0
41
42 #angular velocity in the z-axis:
43 vel_msg.angular.x = 0
44 vel_msg.angular.y = 0
45 vel_msg.angular.z = 4 * (atan2(goal_pose.y - self.pose.y,
goal_pose.x - self.pose.x) - self.pose.theta)
46
47 #Publishing our vel_msg
48 self.velocity_publisher.publish(vel_msg)
49 self.rate.sleep()
50 #Stopping our robot after the movement is over
51 vel_msg.linear.x = 0
52 vel_msg.angular.z =0
53 self.velocity_publisher.publish(vel_msg)
54
55 rospy.spin()
56
57 if __name__ == '__main__':
58 try:
59 #Testing our function
60 x = turtlebot()
61 x.move2goal()
62
63 except rospy.ROSInterruptException: pass