ScribblePath/tb_controller.py at main · SAREC-Lab/ScribblePath · GitHub

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
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
#!/usr/bin/env python
# Software License Agreement (BSD License)
# Modified from https://www.theconstructsim.com/ros-qa-053-how-to-move-a-robot-to-a-certain-point-using-twist/
# to work with TurtleBot3-Burger

import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2, sqrt, pi

from sensor_msgs.msg import LaserScan

x = 0.0
y = 0.0
theta = 0.0

stop = False

# Initialize mode
rospy.init_node("speed_controller")
# rospy.init_node('obstacle_avoidance_node')

# Publish linear and angular velocities to cmd_vel topic
pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 10)

speed = Twist()

# Callback function
def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

def obstacle_detection_callback(dt):
    global stop
    global pub

    print '-------------------------------------------'
    print 'Range data at 0 deg:   {}'.format(dt.ranges[0])
    print 'Range data at 15 deg:  {}'.format(dt.ranges[15])
    print 'Range data at 345 deg: {}'.format(dt.ranges[345])
    print '-------------------------------------------'

    # Thresholds
    thr1 = 0.3
    thr2 = 0.3

    # Checks if obstacles in front and 15 deg left and right
    if not (dt.ranges[0] > thr1 and dt.ranges[15] > thr2 and dt.ranges[345] > thr2):
        speed.linear.x = 0.0
        speed.angular.z = 0.0
        stop = True
        print("OBSTACLE!!!")

    pub.publish(speed)

# Subscribe to the odom topic to get information about the current position and velocity
# of the robot
sub = rospy.Subscriber("/odom", Odometry, newOdom)
sub1 = rospy.Subscriber("/scan", LaserScan, obstacle_detection_callback)

def turtle(waypoints):
    global stop

    r = rospy.Rate(4)

    # Establish target coordinates
    goal = Point()
    current_goal=0
    # waypoints = [[5,5],[-6,1],[3,8],[10,0]]
    goal.x=waypoints[current_goal][0]
    goal.y=waypoints[current_goal][1]
    # Strategy is to first turn to face the target coordinates
    # and then move towards them
    while not rospy.is_shutdown() and not stop:

        # Compute difference between current position and target position
        inc_x = goal.x -x
        inc_y = goal.y -y
        angle_to_goal = atan2(inc_y, inc_x)

        difference_angle = abs(angle_to_goal - theta)
        msg2 = ("x: {},  y: {}, velocity: {}, angle: {}".format(x,y,speed.linear.x,theta))
        rospy.loginfo(msg2)
        dist = sqrt(inc_x**2 + inc_y**2)
        # Check whether to turn or move.
        angle_change=angle_to_goal-theta
        if dist<.1:
            speed.angular.z=0
            speed.linear.x=0
            if current_goal +1 < len(waypoints):
                current_goal+=1
                goal.x=waypoints[current_goal][0]
                goal.y=waypoints[current_goal][1]
        elif abs(angle_change) > 0.25:
            #speed.linear.x = 0.0
            if angle_change > 0:
                speed.angular.z = 0.50

                if angle_change > pi/8:
                    speed.angular.z = 0.75

                if angle_change > pi/4:
                    speed.angular.z = 1.00

                if angle_change > pi/2:
                    speed.angular.z = 1.25

                if angle_change > 3*pi/4:
                    speed.angular.z = 1.50

                if angle_change > 3*pi/4 + ((3*pi/4) + pi)/2:
                    speed.angular.z = 1.75
            else:
                speed.angular.z = -0.50

                if angle_change < -pi/8:
                    speed.angular.z = -0.75

                if angle_change < -pi/4:
                    speed.angular.z = -1.00

                if angle_change < -pi/2:
                    speed.angular.z = -1.25

                if angle_change < -3*pi/4:
                    speed.angular.z = -1.50

                if angle_change < -(3*pi/4 + ((3*pi/4) + pi)/2):
                    speed.angular.z = -1.70
        else:
            speed.angular.z = 0.0

            rospy.loginfo("Ready to move")

            speed.linear.x = 0.300

            if dist > 0.25:
                speed.linear.x = 0.350

            if dist > 0.50:
                speed.linear.x = 0.400

            if dist > 0.75:
                speed.linear.x = 0.450

            if dist > 1:
                speed.linear.x = 0.500

            if dist > 1.25:
                speed.linear.x = 0.550

            if dist > 1.5:
                speed.linear.x = 0.600

            # speed.linear.x = dist*0.05+.025
            # speed.angular.z = 0.0

        pub.publish(speed)
        r.sleep()

def main():
    waypoints = [[1,1], [1,1.1], [1,1.2], [2,0], [3,1], [4,0]]
    turtle(waypoints)

if __name__ == "__main__":
    main()