class Navigation(object):
def __init__(self):
self.connection = LabNavigation()
self.path_planner = GapFinder(.7)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(1)
self.space = .9
self.target_x = 0
self.target_y = 2
sleep(5)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity=0.0
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def move(self, data):
distances = list(data.ranges)[0::every_other]
self.path_planner.filterReadings(distances, angles)
closest_reading = self.path_planner.getMinimumReading()
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity+(closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time))/2
self.crash_avert_velocity = min(0.0,self.crash_avert_velocity)
controlled_velocity = closest_reading * kp + self.crash_avert_velocity
controlled_velocity = max(0.0,min(controlled_velocity,1))
print 'Controlled Velocity:', controlled_velocity,
print 'closest_reading:',closest_reading,
print 'Crash avert velocity:',self.crash_avert_velocity
self.actuation.setTangentialVelocityLimit(controlled_velocity)
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(1)
# print 'agent location (x,y):', x, y, yaw
diff_x = self.target_x - x
diff_y = self.target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
if self.distance < .03:
self.target_y = self.target_y * -1
angle = arctan2(diff_y, diff_x) - yaw
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
self.tracker.moveTowardsDynamicPoint(subgoal_distance, -subgoal_angle)
self.prev_closest_reading = closest_reading
self.prev_time = time_now
class Navigation(object):
def __init__(self):
sleep(5)
self.distance = []
self.connection = NaslabNetwork()
self.path_planner = GapFinder(.5)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(0)
self.target_body_number = 2
self.space = .75
self.direction = 1
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
# def start(self):
def move(self, data):
distances = list(data.ranges)[0::every_other]
self.path_planner.filterReadings(distances, angles)
x, y, theta = self.connection.getStates(0)
target_x, target_y, target_theta = self.connection.getStates(self.target_body_number)
target_x = target_x + self.space * cos(target_theta)
target_y = target_y + self.space * sin(target_theta)
diff_x = target_x - x
diff_y = target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
if self.distance < .03:
if self.target_body_number == 1:
self.tracker.faceDirection(target_theta)
print 'ARRIVED!!!!!!!!!!!'
self.subscriber.unregister()
return
if self.target_body_number == 2:
self.tracker.faceDirection(pi+target_theta)
self.target_body_number = 1
self.space = -1.1
print 'ARRIVED!!!!!!!!!!!'
sleep(4)
angle = arctan2(diff_y, diff_x) - theta
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
self.tracker.moveTowardsDynamicPoint(subgoal_distance, -subgoal_angle)
def __init__(self):
# Set parameters
self.flag = 1
self.gap = .7 #space needed to pass through
self.agent_id = 1 #Number for Robot $$change
self.stage = 0 #initial target
self.substage = 0 #initial task 0 = drive to objective
self.topicConfig() #configure simulation namspaces or actual huskies
self.connection = gpsLocalization(self.namespace, self.gpsTopic, self.imuTopic, self.xOffset, self.yOffset) #Connection which will give current position
self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
self.actuation = ROS2DimActuate(self.controlTopic) #Controls the motion of the robot
#Create a tracker which knows how to move the robot and get it's position
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
#Tell the tracker which robot to command
self.tracker.setID(self.agent_id)
#Countdown to start
sleep_time = 3
while sleep_time > 0:
print "Mission starts in:", sleep_time
sleep_time -= 1
sleep(1)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan
rospy.spin()
def __init__(self):
self.gap = .6
# self.space = .9
self.target_x = .5
self.target_y = 1
self.agent_id = 0
self.connection = LabNavigation()
self.path_planner = GapFinder(self.gap)
self.actuation = ROS2DimActuate()
self.actuation.setAngularVelocityLimit(1)
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(self.agent_id)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
sleep(7)
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def __init__(self):
#set parameters
self.gap = .7 #space needed to pass through
self.target_x = 5 #destination coordinates
self.target_y = 4
self.agent_id = 0
self.connection = gpsLocalization() #Connection which will give current position
self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
self.actuation = ROS2DimActuate() #Controls the motion of the robot
self.actuation.setAngularVelocityLimit(.5) #Sets the maximum velocity
#Create a tracker which knows how to move the robot and get it's position
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
#Tell the tracker which robot to command
self.tracker.setID(self.agent_id)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
sleep(2)
self.subscriber = rospy.Subscriber('/husky1/scan', LaserScan, self.move, queue_size=1) #Call move for each laser scan
rospy.spin()
def __init__(self):
self.gap = .7
self.agent_id = 1
self.stage = 0
self.substage = 0
self.connection = LabNavigation()
self.path_planner = GapFinder(self.gap)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(self.agent_id)
sleep_time = 7
while sleep_time > 0:
print "Mission starts in:", sleep_time
sleep_time -= 1
sleep(1)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
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 __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()
def __init__(self):
sleep(5)
self.distance = []
self.connection = NaslabNetwork()
self.path_planner = GapFinder(.5)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(0)
self.target_body_number = 2
self.space = .75
self.direction = 1
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def __init__(self):
self.connection = LabNavigation()
self.path_planner = GapFinder(.7)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(1)
self.space = .9
self.target_x = 0
self.target_y = 2
sleep(5)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity=0.0
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
class Navigation(object):
def __init__(self):
# Set parameters
self.flag = 1
self.gap = .7 #space needed to pass through
self.agent_id = 1 #Number for Robot $$change
self.stage = 0 #initial target
self.substage = 0 #initial task 0 = drive to objective
self.topicConfig() #configure simulation namspaces or actual huskies
self.connection = gpsLocalization(self.namespace, self.gpsTopic, self.imuTopic, self.xOffset, self.yOffset) #Connection which will give current position
self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
self.actuation = ROS2DimActuate(self.controlTopic) #Controls the motion of the robot
#Create a tracker which knows how to move the robot and get it's position
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
#Tell the tracker which robot to command
self.tracker.setID(self.agent_id)
#Countdown to start
sleep_time = 3
while sleep_time > 0:
print "Mission starts in:", sleep_time
sleep_time -= 1
sleep(1)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
self.subscriber = rospy.Subscriber(self.lidarTopic, LaserScan, self.move, queue_size=1) #Call move for each laser scan
rospy.spin()
def topicConfig(self):
if len(argv)>1:
self.namespace = argv[3]
self.lidarTopic = '/' + argv[3] + '/scan'
self.gpsTopic = '/' + argv[3] + '/odometry/gps'
self.imuTopic = '/' + argv[3] + '/imu/data'
self.controlTopic = '/' + argv[3] + '/cmd_vel'
self.xOffset = float(argv[1])
self.yOffset = float(argv[2])
else:
self.namespace = ''
self.lidarTopic ='/scan'
self.gpsTopic = '/navsat/enu'
self.imuTopic = '/imu/data'
self.controlTopic = '/cmd_vel'
self.xOffset = 0
self.yOffset = 0
def move(self, data):
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
print '-----------------------------'
# extract distance data and analyze them
distances = list(data.ranges)[0::every_other]
#Drive to the setpoint for the target
if self.substage == 0:
self.path_planner.filterReadings(distances, angles)
closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()
# dynamic obstacle collision avoidance
closest_reading = min(closest_reading, 2 * self.gap)
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)
# set velocity based on dynamic obstacle movement
controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))
# find destination and analyze it
#get information about the current target
#target_object = self.connection.getStates(targets[self.stage][0])
if self.stage == 0:
target_object = [0,-5,0,0,0]
# Find where to approach target from
target = withDistance(target_object[1], target_object[2], target_object[4], targets[self.stage][1][0])
print target
target_x = target[0]
target_y = target[1]
diff_x = target_x - x
diff_y = target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
# plan path to the target
angle = arctan2(diff_y, diff_x) - yaw # find direction towards target in robots coordinate frame
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
subgoal_angle2 = -subgoal_angle
# go to the point designated by path planner
self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
# See if reached the destination
if self.distance < .1:
print '\033[92m' + '\033[1m' + 'ARRIVED TO GATE' + '\033[0m'
# face direction
if targets[self.stage][1][0] < 0:
desired_facing = 0
#desired_facing = self.connection.getStates(targets[self.stage][0])[4]
else:
desired_facing = pi
#desired_facing = pi + self.connection.getStates(targets[self.stage][0])[4]
self.tracker.faceDirection(desired_facing)
self.substage = 1
sleep(1)
# save some of the variable needed for next iteration
self.prev_closest_reading = closest_reading
self.prev_time = time_now
#Plot the results
# xpol, ypol = self.path_planner.polarToCartesian()
# 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] = x + self.path_planner.readings_polar[i][0] * cos(yaw - self.path_planner.readings_polar[i][1])
# reading_y[i] = y + self.path_planner.readings_polar[i][0] * sin(yaw - self.path_planner.readings_polar[i][1])
# xpol[i] = x + self.path_planner.possible_travel[i] * cos(-self.path_planner.readings_polar[i][1] + yaw)
# ypol[i] = y + self.path_planner.possible_travel[i] * sin(-self.path_planner.readings_polar[i][1] + yaw)
# if i in self.path_planner.subgoals:
# subgoal_x = subgoal_x + [xpol[i]]
# subgoal_y = subgoal_y + [ypol[i]]
# if self.flag:
# self.f0 = plt.figure(1,figsize=(9,9))
# self.ax0 = self.f0.add_subplot(111)
# self.sgc, = self.ax0.plot(subgoal_x, subgoal_y, 'ko', markersize=20, label='Subgoal Candidate')
# self.bsg, = self.ax0.plot(x + subgoal_distance * cos(yaw - subgoal_angle),
# y + subgoal_distance * sin(yaw - subgoal_angle), 'go', markersize=20, label='Best Subgoal')
# self.ro, = self.ax0.plot(x, y, 'ms', markersize=10, label='Robot')
# self.dest, = self.ax0.plot(target_x, target_y, 'cs', markersize=10, label='Destination')
# self.pot, = self.ax0.plot(xpol, ypol, 'b.', markersize=10, label='Possible Travel')
# self.lidr, = self.ax0.plot(reading_x, reading_y, 'r.', markersize=10, label='Lidar Reading')
# self.ax0.set_xlabel('X (m)')
# self.ax0.set_ylabel('Y (m)')
# self.ax0.legend()
# self.ax0.axis('equal')
# plt.tight_layout()
# plt.draw()
# plt.pause(.1)
# self.flag = 0
# else:
# self.sgc.set_xdata(subgoal_x)
# self.sgc.set_ydata(subgoal_y)
# self.bsg.set_xdata(x + subgoal_distance * cos(yaw - subgoal_angle))
# self.bsg.set_ydata(y + subgoal_distance * sin(yaw - subgoal_angle))
# self.ro.set_xdata(x)
# self.ro.set_ydata(y)
# self.dest.set_xdata(target_x)
# self.dest.set_ydata(target_y)
# self.pot.set_xdata(xpol)
# self.pot.set_ydata(ypol)
# self.lidr.set_xdata(reading_x)
# self.lidr.set_ydata(reading_y)
# plt.draw()
#approach the target
elif self.substage == 1:
return
front_travel = self.path_planner.getFrontTravel(distances, angles)
front_error = front_travel - targets[self.stage][1][1]
print front_travel, front_error
if abs(front_error) < .03:
self.substage = 2
print 'Connection made. Reversing.'
sleep(5)
self.actuation.actuate(.5 * front_error, 0)
#reverse a set distance
elif self.substage == 2:
front_travel = self.path_planner.getFrontTravel(distances, angles)
front_error = front_travel - targets[self.stage][1][1] - 1
print front_travel, front_error
if abs(front_error) < .03:
self.stage += 1
self.substage = 0
print 'Connection complete. Proceeding to next operation.'
self.actuation.actuate(.5 * front_error, 0)
if self.stage == len(targets):
save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/planner_of_agent_' + str(self.agent_id), log)
self.subscriber.unregister()
print '\033[92m' + '\033[1m' + 'AND DONE' + '\033[0m'
elif self.stage > len(targets): # just do nothing after that
print "didn't unregister"
sleep(100)
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 Navigation(object):
def __init__(self):
self.gap = .7
self.agent_id = 1
self.stage = 0
self.substage = 0
self.connection = LabNavigation()
self.path_planner = GapFinder(self.gap)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(self.agent_id)
sleep_time = 7
while sleep_time > 0:
print "Mission starts in:", sleep_time
sleep_time -= 1
sleep(1)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def move(self, data):
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
print '-----------------------------'
global i
global finished_edge
# extract distance data and analyze them
distances = list(data.ranges)[0::every_other]
if self.substage == 0:
self.path_planner.filterReadings(distances, angles)
closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()
# dynamic obstacle collision avoidance
closest_reading = min(closest_reading, 2 * self.gap)
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)
# set velocity based on dynamic obstacle movement
controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))
# find destination and analyze it
target_object = self.connection.getStates(targets[self.stage][0])
target = withDistance(target_object[1], target_object[2], target_object[4], targets[self.stage][1][0])
# print target
target_x = target[0]
target_y = target[1]
diff_x = target_x - x
diff_y = target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
print 'here1'
# plan path to the target
angle = arctan2(diff_y, diff_x) - yaw # find direction towards target in robots coordinate frame
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
subgoal_angle2 = -subgoal_angle
# go to the point designated by path planner
self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
# See if reached the destination
if self.distance < .1:
print '\033[92m' + '\033[1m' + 'ARRIVED TO GATE' + '\033[0m'
# face direction
if targets[self.stage][1][0] < 0:
desired_facing = self.connection.getStates(targets[self.stage][0])[4]
else:
desired_facing = pi + self.connection.getStates(targets[self.stage][0])[4]
self.tracker.faceDirection(desired_facing)
self.substage = 1
sleep(1)
# save some of the variable needed for next iteration
self.prev_closest_reading = closest_reading
self.prev_time = time_now
elif self.substage == 1:
front_travel = self.path_planner.getFrontTravel(distances, angles)
front_error = front_travel - targets[self.stage][1][1]
print front_travel, front_error
if abs(front_error) < .03:
self.substage = 2
print 'BREAKINGGGGGGGGGGGGGGGGGGGG'
sleep(5)
self.actuation.actuate(.5 * front_error, 0)
elif self.substage == 2:
front_travel = self.path_planner.getFrontTravel(distances, angles)
front_error = front_travel - targets[self.stage][1][1] - .2
print front_travel, front_error
if abs(front_error) < .03:
self.stage += 1
self.substage = 0
print 'BREAKINGGGGGGGGGGGGGGGGGGGG'
self.actuation.actuate(.5 * front_error, 0)
if self.stage == len(targets):
save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/planner_of_agent_' + str(self.agent_id), log)
self.subscriber.unregister()
print '\033[92m' + '\033[1m' + 'AND DONE' + '\033[0m'
elif self.stage > len(targets): # just do nothing after that
print "Stupid shit didn't unregister"
sleep(100)
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
class Navigation(object):
def __init__(self):
#set parameters
self.gap = .7 #space needed to pass through
self.target_x = 5 #destination coordinates
self.target_y = 4
self.agent_id = 0
self.connection = gpsLocalization() #Connection which will give current position
self.path_planner = GapFinder(self.gap) #Finds gaps that the robot can enter
self.actuation = ROS2DimActuate() #Controls the motion of the robot
self.actuation.setAngularVelocityLimit(.5) #Sets the maximum velocity
#Create a tracker which knows how to move the robot and get it's position
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
#Tell the tracker which robot to command
self.tracker.setID(self.agent_id)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
sleep(2)
self.subscriber = rospy.Subscriber('/husky1/scan', LaserScan, self.move, queue_size=1) #Call move for each laser scan
rospy.spin()
def move(self, data):
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id) #Get localization info
#print 'x: ', x,'y: ', y,'theta: ', yaw
print '-----------------------------'
global i
global stage
global finished_logging
distances = list(data.ranges)[0::every_other] #store the range readings from the lidar
self.path_planner.filterReadings(distances, angles) #filter the results
closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()
closest_reading = min(closest_reading, 2 * self.gap)
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)
controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
self.actuation.setTangentialVelocityLimit(min(.2, controlled_velocity))
i += 1
if i % temp_var is 0 and i < temp_var_2:
log[i / temp_var] = [x, y, yaw, self.path_planner.readings_polar]
diff_x = self.target_x - x
diff_y = self.target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
if self.distance < .1:
stage += 1
print 'ARRIVED!!!!!!!!!!'
if finished_logging is False and i >= temp_var_2:
self.tracker.saveLog()
save('loginfo', log)
finished_logging = True
self.target_y = self.target_y * -1
self.target_x = self.target_x * -1
exit()
angle = arctan2(diff_y, diff_x) - yaw
#print 'dist: ', self.distance, 'angle: ', -angle
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
subgoal_angle2 = -subgoal_angle
self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
self.prev_closest_reading = closest_reading
self.prev_time = time_now
def simple_gap_finder():
finder = GapFinder(.5)
finder.setDistanceRange(5)
return finder
class Navigation(object):
def __init__(self):
self.gap = .6
# self.space = .9
self.target_x = .5
self.target_y = 1
self.agent_id = 0
self.connection = LabNavigation()
self.path_planner = GapFinder(self.gap)
self.actuation = ROS2DimActuate()
self.actuation.setAngularVelocityLimit(1)
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(self.agent_id)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
print 'Starting the Navigation'
sleep(7)
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def move(self, data):
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
# sleep(1)
print '-----------------------------'
global i
global stage
global finished_logging
distances = list(data.ranges)[0::every_other]
self.path_planner.filterReadings(distances, angles)
closest_reading, closest_reading_angle = self.path_planner.getMinimumReading()
closest_reading = min(closest_reading, 2 * self.gap)
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)
# print 'Crash avert velocity:% 4.2f'%self.crash_avert_velocity
controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
# print 'Controlled Velocity:', controlled_velocity,
# print 'closest_reading:',closest_reading,
# print 'Crash avert velocity:',self.crash_avert_velocity
self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))
i += 1
if i % temp_var is 0 and i < temp_var_2:
log[i / temp_var] = [x, y, yaw, self.path_planner.readings_polar]
diff_x = self.target_x - x
diff_y = self.target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
# print 'distance',self.distance
if self.distance < .1:
stage += 1
print 'ARRIVED!!!!!!!!!!'
if finished_logging is False and i >= temp_var_2:
self.tracker.saveLog()
save('/home/administrator/barzin_catkin_ws/src/path_tracking/scripts/experimental_results/env', log)
finished_logging = True
if stage % 2 is 0:
self.target_y = self.target_y * -1
else:
self.target_x = self.target_x * -1
# exit()
angle = arctan2(diff_y, diff_x) - yaw
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
subgoal_angle2 = -subgoal_angle
# print angle,subgoal_angle2
# faz = 1
# var = min(max(0,self.gap*(1+faz)-closest_reading),faz)
# offset = var*pi/faz/4
# subgoal_angle2 = subgoal_angle2+offset*sign(subgoal_angle2-(-closest_reading_angle))
# print '% 4.2f, % 4.2f, % 4.2f' % (var, offset,offset*sign(subgoal_angle2-(-closest_reading_angle)))
# print self.distance,-angle,subgoal_distance,subgoal_angle2
self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
# print 'target angle:',yaw+subgoal_angle2
self.prev_closest_reading = closest_reading
self.prev_time = time_now
class Navigation(object):
def __init__(self):
self.gap = .9
# self.space = .9
self.target_x = 0
self.target_y = 2
self.agent_id = 1
self.connection = LabNavigation()
self.path_planner = GapFinder(self.gap)
self.actuation = ROS2DimActuate()
self.tracker = PlanarTracker(self.actuation.actuate, self.connection.getStates)
self.tracker.setID(self.agent_id)
sleep(5)
self.distance = []
self.prev_closest_reading = 0.0
self.prev_time = time()
self.crash_avert_velocity = 0.0
self.subscriber = rospy.Subscriber('/scan', LaserScan, self.move, queue_size=1)
rospy.spin()
def move(self, data):
distances = list(data.ranges)[0::every_other]
self.path_planner.filterReadings(distances, angles)
closest_reading,closest_reading_angle = self.path_planner.getMinimumReading()
closest_reading = min(closest_reading,2*self.gap)
time_now = time()
self.crash_avert_velocity = (self.crash_avert_velocity + (closest_reading - self.prev_closest_reading) * kd / (time() - self.prev_time)) / 2
self.crash_avert_velocity = min(0.0, self.crash_avert_velocity)
# print 'Crash avert velocity:% 4.2f'%self.crash_avert_velocity
controlled_velocity = (closest_reading) * kp + self.crash_avert_velocity
controlled_velocity = max(0.0, min(controlled_velocity, 1.0))
# print 'Controlled Velocity:', controlled_velocity,
# print 'closest_reading:',closest_reading,
# print 'Crash avert velocity:',self.crash_avert_velocity
self.actuation.setTangentialVelocityLimit(min(1, controlled_velocity))
agent_id, x, y, z, yaw, pitch, roll = self.connection.getStates(self.agent_id)
# print 'agent location (x,y):', x, y, yaw
diff_x = self.target_x - x
diff_y = self.target_y - y
self.distance = sqrt(diff_x**2 + diff_y**2)
# print 'distance',self.distance
if self.distance < .1:
print 'ARRIVED!!!!!!!!!!'
self.target_y = self.target_y * -1
angle = arctan2(diff_y, diff_x) - yaw
subgoal_distance, subgoal_angle = self.path_planner.planPath(self.distance, -angle)
subgoal_angle2 = -subgoal_angle
# print angle,subgoal_angle2
# faz = 1
# var = min(max(0,self.gap*(1+faz)-closest_reading),faz)
# offset = var*pi/faz/4
# subgoal_angle2 = subgoal_angle2+offset*sign(subgoal_angle2-(-closest_reading_angle))
# print '% 4.2f, % 4.2f, % 4.2f' % (var, offset,offset*sign(subgoal_angle2-(-closest_reading_angle)))
self.tracker.moveTowardsDynamicPoint(subgoal_distance, subgoal_angle2)
# print 'target angle:',yaw+subgoal_angle2
self.prev_closest_reading = closest_reading
self.prev_time = time_now
