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
class LidarLogger(object):
def __init__(self):
self.path_planner = GapFinder(.6)
self.logger = []
self.connection = gpsLocalization('husky1test','husky1/odometry/gps','husky1/imu/data',0,0)
self.subscriber = rospy.Subscriber('husky1/scan', LaserScan, self.readDistance)
self.rate = rospy.Rate(10) # set rate to 10 Hz
while not rospy.is_shutdown() and len(self.logger) < 1:
self.rate.sleep()
def readDistance(self, data):
self.subscriber.unregister()
distances = list(data.ranges)[0::every_other]
self.path_planner.filterReadings(distances, angles)
x, y = self.path_planner.polarToCartesian()
crap1, robot_x, robot_y, robot_z, robot_yaw, crap, crap2 = self.connection.getStates(0)
target_x = 7
target_y = 4
diff_x = target_x - robot_x
diff_y = target_y - robot_y
print robot_x
distance = sqrt(diff_x**2 + diff_y**2)
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
f0 = plt.figure(1,figsize=(9,9))
ax0 = f0.add_subplot(111)
nums = len(self.path_planner.possible_travel)
reading_x = [0] * nums
reading_y = [0] * nums
subgoal_x = []
subgoal_y = []
for i in range(len(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:
subgoal_x = subgoal_x + [x[i]]
subgoal_y = subgoal_y + [y[i]]
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
