-
Notifications
You must be signed in to change notification settings - Fork 1
/
move.py
52 lines (45 loc) · 1.56 KB
/
move.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
def move():
# Starts a new node
rospy.init_node('robot_cleaner', anonymous=True)
velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
vel_msg = Twist()
#Receiveing the user's input
print("Let's move your robot")
speed = input("Input your speed:")
distance = input("Type your distance:")
isForward = input("Foward?: ")#True or False
#Checking if the movement is forward or backwards
if(isForward):
vel_msg.linear.x = abs(speed)
else:
vel_msg.linear.x = -abs(speed)
#Since we are moving just in x-axis
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
vel_msg.angular.z = 0
while not rospy.is_shutdown():
#Setting the current time for distance calculus
t0 = rospy.Time.now().to_sec()
current_distance = 0
#Loop to move the turtle in an specified distance
while(current_distance < distance):
#Publish the velocity
velocity_publisher.publish(vel_msg)
#Takes actual time to velocity calculus
t1=rospy.Time.now().to_sec()
#Calculates distancePoseStamped
current_distance= speed*(t1-t0)
#After the loop, stops the robot
vel_msg.linear.x = 0
#Force the robot to stop
velocity_publisher.publish(vel_msg)
if __name__ == '__main__':
try:
#Testing our function
move()
except rospy.ROSInterruptException: pass