def controller_callback(self):
print('send command')
msg = RawControlCommand()
if self.cmd == False:
if self.dis <= 15:
msg.brake = 1000 * int(40.0 / self.dis)
else:
msg.brake = 750 * int(40.0 / self.dis)
elif self.twist <= 11 and self.cmd == True:
msg.throttle = 70
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
if self.dis < 6.0:
msg.brake = 750
self.pub.publish(msg)
print('brake')
elif self.old_dis < 10.0 and self.old_dis > self.dis:
msg.brake = 400
self.pub.publish(msg)
print('approaching')
elif self.dis >= 6.0 and self.old_dis < self.dis:
msg.throttle = 80
self.pub.publish(msg)
print('speed up with speed %d' % msg.throttle)
self.old_dis = self.dis
def controller_callback(self):
#print(self.accelerate)
msg = RawControlCommand()
a = (self.dist - self.last_dist) * 10
print('dist: %d, a: %d' % (self.dist, a))
if self.dist >= 5 and self.dist <= 7 and a > -10:
msg.throttle = 50
msg.brake = 0
elif self.dist + a < 5:
msg.throttle = 0
msg.brake = 100
print('brake')
elif self.dist > 7:
msg.throttle = 60
msg.brake = 0
else:
msg.throttle = 0
msg.brake = 40
print('else brake')
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = 20
msg.front_steer = 0
dis = 0
if self.init and self.now:
dis = abs(self.init.x - self.now.x)
if self.velocity and ((self.velocity.x**2) + (self.velocity.y**2) +
(self.velocity.z**2))**0.5 > 10.0 / 3.6:
msg.throttle = 0
if dis > 17 and (self.left < 20000 or self.right < 20000):
if self.turn < 0 and self.left:
msg.front_steer = int(-50 * 1000.0 / float(self.left))
elif self.turn > 0 and self.right:
msg.front_steer = int(30 * 1000.0 / float(self.right))
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = 30
msg.front_steer = 0
if self.position and self.pre_position and self.velocity and self.currentCheckPoint < len(
checkPoints):
if ((self.velocity.x**2) + (self.velocity.y**2) +
(self.velocity.z**2))**0.5 > 30.0 / 3.6:
msg.throttle = 0
dest_vec = np.array([
checkPoints[self.currentCheckPoint].x - self.position.x,
checkPoints[self.currentCheckPoint].y - self.position.y
])
now_vec = np.array([
self.position.x - self.pre_position.x,
self.position.y - self.pre_position.y
])
dest_vec /= dest_vec.dot(dest_vec)**0.5
if now_vec.dot(now_vec) == 0:
now_vec = np.array([0, 0])
else:
now_vec /= now_vec.dot(now_vec)**0.5
dest_rot = np.array([
now_vec[0] * dest_vec[0] + now_vec[1] * dest_vec[1],
-now_vec[1] * dest_vec[0] + now_vec[0] * dest_vec[1]
])
angle = -dest_rot[1]
print('waypoint = %d' % (self.currentCheckPoint))
print('dest = %f %f' % (dest_vec[0], dest_vec[1]))
print('now = %f %f' % (now_vec[0], now_vec[1]))
print('angle = %f' % (angle))
msg.front_steer = int(100 * (angle))
if self.currentCheckPoint >= len(checkPoints):
msg.throttle = 0
msg.brake = 50
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = int(self.throttle)
msg.front_steer = int(self.steer)
msg.brake = int(self.brake)
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = 30
msg.front_steer = 0
if self.position and self.pre_position and len(
checkPoints) > self.currentCheckPoint:
checkPoint = checkPoints[self.currentCheckPoint]
#print('x = %d, y = %d, task = %d' %(checkPoint.x, checkPoint.y, checkPoint.task))
if self.currentTask == 0: # Direct by waypoints
dest_vec = np.array([
checkPoints[self.currentCheckPoint].x - self.position.x,
checkPoints[self.currentCheckPoint].y - self.position.y
])
now_vec = np.array([
self.position.x - self.pre_position.x,
self.position.y - self.pre_position.y
])
dest_vec /= dest_vec.dot(dest_vec)**0.5
if now_vec.dot(now_vec) == 0:
now_vec = np.array([0, 0])
else:
now_vec /= now_vec.dot(now_vec)**0.5
dest_rot = np.array([
now_vec[0] * dest_vec[0] + now_vec[1] * dest_vec[1],
-now_vec[1] * dest_vec[0] + now_vec[0] * dest_vec[1]
])
angle = -dest_rot[1]
'''
print('waypoint = %d' %(self.currentCheckPoint))
print('dest = %f %f' %(dest_vec[0], dest_vec[1]))
print('now = %f %f' %(now_vec[0], now_vec[1]))
print('angle = %f' %(angle))
'''
msg.front_steer = int(100 * (angle))
'''
elif self.currentTask == 1: # Traffic Light
msg.throttle, msg.brake, msg.front_steer = task1.start()
elif self.currentTask == 2: # Road Curve
msg.throttle, msg.brake, msg.front_steer = task2.start()
'''
elif self.currentTask == 3: # Narrowing Driving Lanes
msg.throttle, msg.brake, msg.front_steer = task3.start(
self.avg, checkPoints[taskToCheckPoints[3]], self.position,
self.rightx, self.righty)
elif self.currentTask == 4: # T-junction
msg.throttle, msg.brake, msg.front_steer = task4.start(
checkPoints[taskToCheckPoints[4] + 1],
checkPoints[taskToCheckPoints[4]], self.position,
self.pre_position, self.thru, self.rightx)
elif self.currentTask == 5: # Roundabout
msg.throttle, msg.brake, msg.front_steer = task5.start(
self.avg, checkPoints[self.currentCheckPoint],
checkPoints[taskToCheckPoints[5]],
checkPoints[taskToCheckPoints[5] + 1],
checkPoints[taskToCheckPoints[5] + 2], self.position,
self.pre_position)
'''
elif self.currentTask == 6: # Angled Parking Slots
msg.throttle, msg.brake, msg.front_steer = task6.start()
'''
# Speed limit to 10 m/s
if ((self.velocity.x**2) + (self.velocity.y**2) +
(self.velocity.z**2))**0.5 > 10.0 / 3.6:
msg.throttle = 0
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = 100
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = 0
msg.brake = 0
if self.init + 13 > self.position:
if self.init + 9 > self.position:
msg.throttle = 50
elif self.distance > 6:
msg.throttle = 10
else:
msg.brake = 20
elif self.init + 30 < self.position:
if self.velocity > 5.0 / 3.6:
msg.brake = 50
else:
msg.brake = 25
elif self.velocity > 10.0 / 3.6:
msg.brake = 25
elif self.velocity < 5.0 / 3.6:
msg.throttle = 20
else:
if self.distance > 18:
msg.throttle = 50
elif self.distance > 12:
msg.throttle = 20
#elif self.distance > 12:
#msg.throttle = 0
elif self.distance > 10:
msg.brake = 20
else:
msg.brake = 50
#print('throttle = %2f, brake = %2f' %(msg.throttle, msg.brake))
self.pub.publish(msg)
def controller_callback(self):
msg = RawControlCommand()
msg.throttle = self.throttle
msg.front_steer = self.steer
self.pub.publish(msg)
def controller_callback(self):
#TODO
msg = RawControlCommand()
msg.throttle = 10
'''
if self.info_bounding_boxes_data_new is not None:
for box in self.info_bounding_boxes_data_new.boxes:
print('central of box is at %f %f %f.' % (box.centroid.x, box.centroid.y, box.centroid.z))
'''
if self.info_odom_data_new is not None:
position = self.info_odom_data_new.pose.pose.position
self.info_current_position = np.array(
[position.x, position.y, position.z])
#print('Current pos:',self.info_current_position)
# detect and act
if (self.info_odom_data_new is not None) and (
self.info_bounding_boxes_data_new
is not None) and (self.info_odom_data_old is not None) and (
self.info_bounding_boxes_data_old is not None):
position_new = self.info_odom_data_new.pose.pose.position
position_old = self.info_odom_data_old.pose.pose.position
self.info_direction_vector = np.array([
position_new.x - position_old.x,
position_new.y - position_old.y,
position_new.z - position_old.z
])
info_direction_vector_length = np.linalg.norm(
self.info_direction_vector)
self.info_direction_vector_avg = (
self.info_direction_vector +
self.info_direction_vector_old) / 2
if info_direction_vector_length != 0:
self.info_direction_vector_old = self.info_direction_vector
else:
self.info_direction_vector = self.info_direction_vector_old
#theta = math.atan(self.info_direction_vector_avg[1]/self.info_direction_vector[0])
#print("direction_vector_avg: ", self.info_direction_vector_avg, self.info_direction_vector_avg[0]/self.info_direction_vector_avg[1])
if np.linalg.norm(self.info_direction_vector_avg) != 0:
for box in self.info_bounding_boxes_data_new.boxes:
#self.counter = self.counter + 1
# item
item_vector = np.array(
[box.centroid.x, box.centroid.y, box.centroid.z])
item_correct_vector = item_vector + self.info_lidar_offset
item_correct_vector_no_z = np.array(
[item_correct_vector[0], item_correct_vector[1], 0])
if np.linalg.norm(
item_correct_vector_no_z
) < 30 and item_correct_vector[2] > 0.25 and abs(
box.centroid.y) < 2:
print("Almost hit")
self.alert_counter = self.alert_counter - 1
self.plotlist[0].append(box.centroid.x)
self.plotlist[1].append(box.centroid.y)
else:
self.reset_counter - 1
'''
if item_correct_vector[2] > 0.2:
self.plotlist[0].append(box.centroid.x)
self.plotlist[1].append(box.centroid.y)
'''
#if (box.centroid.z > 0.7 and box.centroid.z < 3 and abs(box.centroid.y) < 20 ):
# self.plotlist[0].append(box.centroid.x)
# self.plotlist[1].append(box.centroid.y)
#if (box.centroid.y > self.car_lidar_y - self.info_width_parameter and box.centroid.y < self.car_lidar_y + self.info_width_parameter) and (box.centroid.x > self.car_lidar_x and box.centroid.y < self.car_lidar_x + self.info_alert_distance):
# print("Almost hit")
#print("size: ", box.size)
#print("item",item_vector)
#if CalculateCollisionDetect(item_vector, np.array([1,0,0]), self.info_width_parameter, self.info_alert_distance, self.info_hight_parameter) == 1:
#print("collision alert", "x position: ", box.centroid.x, "y position: ", box.centroid.y)
#print(item_vector)
if self.reset_counter < 0:
self.reset_counter = self.reset_counter_max
self.alert_counter = self.alert_counter_max
if self.alert_counter < 0:
print("brake")
msg.throttle = 0
msg.brake = 100
self.pub.publish(msg)
#print("Ready to plot")
plt.plot(self.plotlist[0], self.plotlist[1], 'ro')
plt.savefig('test.png')
