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 = self.throttle
msg.front_steer = self.steer
self.pub.publish(msg)