-
Notifications
You must be signed in to change notification settings - Fork 0
/
rs_bot_base.py
executable file
·82 lines (64 loc) · 2.06 KB
/
rs_bot_base.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
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
#!/usr/bin/python
from MotorClass import MotorClass
import time
import roslib;
roslib.load_manifest('rs_bot_base')
import rospy
from geometry_msgs.msg import Twist
motor = MotorClass()
def callback(data):
#limit the values between -1 and 1 to have simular results as the turtlebot
if (data.linear.x > 1.0):
data.linear.x = 1.0
if (data.linear.x < -1.0):
data.linear.x = (-1.0)
if (data.angular.z > 1.0):
data.angular.z = 1.0
if (data.angular.z < -1.0):
data.angular.z = (-1.0)
#scale the values back
speed = data.linear.x * 155.0
turnAngle = data.angular.z * 155.0
if (speed > 0):
speed += 100
if (speed < 0):
speed -= 100
if (turnAngle > 0):
turnAngle += 100
if (turnAngle < 0):
turnAngle -= 100
rospy.loginfo(rospy.get_name() + " linear.x %f and angular.z %f", speed, turnAngle)
wheelDistence = 11.0
correction = 3.0
#right = data.linear.x
#left = data.linear.x
#if (data.angular.z < 0):
# right = (data.angular.z*(-1))
# left = (data.angular.z)
# rospy.loginfo( "turn right left: %d and right: %d", left, right)
#if (data.angular.z > 0):
# left = data.angular.z
# right = (data.angular.z*(-1))
# rospy.loginfo( "turn left left: %d and right: %d", left, right)
right = speed - (2.0/wheelDistence)*(turnAngle) * correction
left = speed + (2.0/wheelDistence)*(turnAngle) * correction
if (right > 255.0):
right = 255.0
if (right < -255.0):
right = (-255.0)
if (left > 255.0):
left = 255.0
if (left < -255.0):
left = (-255.0)
motor.set_motor_speed(0, int(left))
motor.set_motor_speed(1, int(right))
motor.set_motor_speed(2, int(left))
motor.set_motor_speed(3, int(right))
motor.change_motor_speed()
rospy.loginfo(rospy.get_name() + " right speed %f left speed %f", right, left)
def listener():
rospy.Subscriber("cmd_vel", Twist, callback)
rospy.spin()
if __name__ == "__main__":
rospy.init_node('rs_bot_base')
listener()