GroundROS/path_planning_online_debug.py at master · NASLab/GroundROS · 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
#!/usr/bin/env python

import matplotlib.pyplot as plt
import rospy
# import numpy as np
from numpy import arange, pi, sqrt, arctan2, sin, cos
# from time import time, sleep
# from datetime import datetime
# import logging
from sensor_msgs.msg import LaserScan  # Messages from Lidar System
# import sys
from sys import path, exit
path.append('src/Modules/')
from path_planning import GapFinder
from communication import LabNavigation


every_other = 1
increment = pi * .5 / 180
angles = arange(-3 * pi / 4, 3 * pi / 4 + increment, increment)[0::every_other]

# logging.basicConfig(filename='control.log', format=50 * '=' +
# '\n%(asctime)s %(message)s', level=logging.DEBUG)  # set up debug logging


class LidarLogger(object):

    def __init__(self):
        self.path_planner = GapFinder(.6)
        rospy.init_node('lidarLogger')  # start the control node
        self.logger = []
        self.connection = LabNavigation()
        self.subscriber = rospy.Subscriber('/scan', LaserScan, self.readDistance)
        self.rate = rospy.Rate(10)  # set rate to 10 Hz
        # read = 0
        while not rospy.is_shutdown() and len(self.logger) < 1:
            self.rate.sleep()
            # rospy.spin()

        # try:
        #     pass
        # self.__run__() #start motion
        # except Exception, e:
            # print e
        #     print "Error during runtime. Exiting."
        #     logging.exception(e)
        # finally:
        #     print "Run complete."
        # print self.logger  # REMOVE LATER
        #     np.save('scan' + str(datetime.now()), self.logger)

    # def __run__(self):
    # sleep(5) #delay before startup
    #     while not rospy.is_shutdown():
    #         self.rate.sleep()

    def readDistance(self, data):
        self.subscriber.unregister()
        distances = list(data.ranges)[0::every_other]
        # print distances
        self.path_planner.filterReadings(distances, angles)
        # print self.path_planner
        x, y = self.path_planner.polarToCartesian()
        crap1, robot_x, robot_y, robot_z, robot_yaw, crap, crap2 = self.connection.getStates(0)
        target_x = -.5
        target_y = 2.7
        diff_x = target_x - robot_x
        diff_y = target_y - robot_y
        distance = sqrt(diff_x**2 + diff_y**2)
        # print 'here'
        # if distance < .1:
            # print '.'
        #     return
        # print 'here'
        angle = arctan2(diff_y, diff_x) - robot_yaw
        subgoal_distance, subgoal_angle = self.path_planner.planPath(distance, -angle)
        print distance, -angle, subgoal_distance, subgoal_angle
        # scan = data.ranges
        # print scan
        # self.logger = scan
        # print "Lidar Scan Recieved. Logging data..."
        f0 = plt.figure(1,figsize=(9,9))
        ax0 = f0.add_subplot(111)
        nums = len(self.path_planner.possible_travel)
        # for i in range(nums):
        # ax0.plot(x, y, 'r.')
        # print self.path_planner.subgoals
        reading_x = [0] * nums
        reading_y = [0] * nums
        subgoal_x = []
        subgoal_y = []
        for i in range(len(self.path_planner.possible_travel)):
            # print robot_x , self.path_planner.possible_travel[i] , -self.path_planner.readings_polar[i][1] , calibrating_theta,robot_yaw
            # print self.path_planner.possible_travel
            reading_x[i] = robot_x + self.path_planner.readings_polar[i][0] * cos(robot_yaw - self.path_planner.readings_polar[i][1])
            reading_y[i] = robot_y + self.path_planner.readings_polar[i][0] * sin(robot_yaw - self.path_planner.readings_polar[i][1])
            x[i] = robot_x + self.path_planner.possible_travel[i] * cos(-self.path_planner.readings_polar[i][1] + robot_yaw)
            y[i] = robot_y + self.path_planner.possible_travel[i] * sin(-self.path_planner.readings_polar[i][1] + robot_yaw)
            if i in self.path_planner.subgoals:
                # print x[i]
                subgoal_x = subgoal_x + [x[i]]
                subgoal_y = subgoal_y + [y[i]]

        # print subgoal_x
        # if environment_state is 'not_safe':
        # elif environment_state is 'safe':
        #     ax0.plot(target_distance * cos(target_angle),
        #              target_distance * sin(target_angle), 'go', markersize=20)
        # elif environment_state is 'close_to_obstacle':
        #     ax0.plot(target_distance * cos(target_angle),
        #              target_distance * sin(target_angle), 'ro', markersize=20)
        ax0.plot(subgoal_x, subgoal_y, 'ko', markersize=20, label='Subgoal Candidate')
        ax0.plot(robot_x + subgoal_distance * cos(robot_yaw - subgoal_angle),
                 robot_y + subgoal_distance * sin(robot_yaw - subgoal_angle), 'go', markersize=20, label='Best Subgoal')
        ax0.plot(robot_x, robot_y, 'ms', markersize=10, label='Robot')
        ax0.plot(target_x, target_y, 'cs', markersize=10, label='Destination')
        ax0.plot(x, y, 'b.', markersize=10, label='Possible Travel')
        ax0.plot(reading_x, reading_y, 'r.', markersize=10, label='Lidar Reading')
        ax0.set_xlabel('X (m)')
        ax0.set_ylabel('Y (m)')
        ax0.legend()
        ax0.axis('equal')
        plt.tight_layout()
        plt.draw()
        plt.pause(.1)
        raw_input("<Hit Enter To Close>")
        plt.close(f0)
        return


if __name__ == "__main__":
    try:
        lidarLog = LidarLogger()
    except rospy.ROSInterruptException, e:
        raise e