autonomous-driving-turtlebot-with-reinforcement-learning/scripts/scan_node.py at main · lukovicaleksa/autonomous-driving-turtlebot-with-reinforcement-learning · 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
#! /usr/bin/env python

import rospy
import matplotlib.pyplot as plt
from datetime import datetime

import sys
sys.path.insert(0, '/home/maestro/catkin_ws/src/master_rad/scripts')

from Qlearning import *
from Lidar import *
from Control import *

ANGLE_MAX = 360 - 1
ANGLE_MIN = 1 - 1
HORIZON_WIDTH = 75

MIN_TIME_BETWEEN_SCANS = 0
MAX_SIMULATION_TIME = float('inf')

if __name__ == '__main__':
    try:
        state_space = createStateSpace()

        rospy.init_node('scan_node', anonymous = False)
        rate = rospy.Rate(10)

        now = datetime.now()
        dt_string_start = now.strftime("%d/%m/%Y %H:%M:%S")
        print('SCAN NODE START ==> ', dt_string_start ,'\r\n')

        scan_time = 0
        count = 0

        t_0 = rospy.Time.now()
        t_start = rospy.Time.now()

        # init timer
        while not (t_start > t_0):
            t_start = rospy.Time.now()

        t = t_start

        # Init figure - real time
        plt.style.use('seaborn-ticks')
        fig = plt.figure(1)
        ax = fig.add_subplot(1,1,1)

        # main loop
        while not rospy.is_shutdown():
            msgScan = rospy.wait_for_message('/scan', LaserScan)

            scan_time = (rospy.Time.now() - t).to_sec()
            sim_time = (rospy.Time.now() - t_start).to_sec()
            count = count + 1

            if scan_time > MIN_TIME_BETWEEN_SCANS:
                print('\r\nScan cycle:', count , '\r\nScan time:', scan_time, 's')
                print('Simulation time:', sim_time, 's')
                t = rospy.Time.now()

                # distances in [m], angles in [degrees]
                ( lidar, angles ) = lidarScan(msgScan)
                ( state_ind, x1, x2 ,x3 ,x4 ) = scanDiscretization(state_space, lidar)

                crash = checkCrash(lidar)
                object_nearby = checkObjectNearby(lidar)

                print('state index:', state_ind)
                print('x1 x2 x3 x4')
                print(x1, '', x2, '', x3, '', x4)
                if crash:
                    print('CRASH !')
                if object_nearby:
                    print('OBJECT NEARBY !')

                lidar_horizon = np.concatenate((lidar[(ANGLE_MIN + HORIZON_WIDTH):(ANGLE_MIN):-1],lidar[(ANGLE_MAX):(ANGLE_MAX - HORIZON_WIDTH):-1]))
                #angles_horizon = np.concatenate((angles[(ANGLE_MIN + HORIZON_WIDTH):(ANGLE_MIN):-1],angles[(ANGLE_MAX):(ANGLE_MAX - HORIZON_WIDTH):-1]))
                angles_horizon = np.linspace(90+HORIZON_WIDTH, 90-HORIZON_WIDTH, 150)

                # horizon in x-y plane
                x_horizon = np.array([])
                y_horizon = np.array([])
                for i in range(len(lidar_horizon)):
                    x_horizon = np.append(x_horizon,lidar_horizon[i] * np.cos(radians(angles_horizon[i])))
                    y_horizon = np.append(y_horizon,lidar_horizon[i] * np.sin(radians(angles_horizon[i])))

                ax.clear()
                plt.xlabel('distance[m]')
                plt.ylabel('distance[m]')
                plt.xlim((-1.0,1.0))
                plt.ylim((-0.2,1.2))
                plt.title('Lidar horizon')
                plt.axis('equal')
                ax.plot(x_horizon, y_horizon, 'b.', markersize = 8, label = 'obstacles')
                ax.plot(0, 0, 'r*', markersize = 20, label = 'robot')
                plt.legend(loc = 'lower right', shadow = True)
                plt.draw()
                plt.pause(0.0001)

            if sim_time > MAX_SIMULATION_TIME:
                now = datetime.now()
                dt_string_stop = now.strftime("%d/%m/%Y %H:%M:%S")
                print('\r\nSCAN NODE START ==> ', dt_string_start ,'\r\n')
                print('SCAN NODE STOP ==> ', dt_string_stop ,'\r\n')
                rospy.signal_shutdown('End of simulation')

            rate.sleep()

    except rospy.ROSInterruptException:
        now = datetime.now()
        dt_string_stop = now.strftime("%d/%m/%Y %H:%M:%S")
        print('\r\nSCAN NODE START ==> ', dt_string_start ,'\r\n')
        print('SCAN NODE STOP ==> ', dt_string_stop ,'\r\n')
        rospy.signal_shutdown('End of simulation')

        pass