def control_callback(msg):
data = CarlaEgoVehicleControl()
data.throttle = msg.velocity
data.steer = msg.angle
data.brake = 0
data.gear = 1
data.reverse = False
data.manual_gear_shift = True
pub.publish(data)
vehicle_control_manual_override_publisher.publish(vehicle_control_manual_override)
auto_pilot_enable_publisher.publish(autopilot_enabled)
def Control(self):
rate = rospy.Rate(10)
lon_param = {
'K_P': 0.5,
'K_I': 0.5,
'K_D': 0
} # Set PID values for longitudinal controller
lat_param = {
'K_P': 0.5,
'K_I': 0.3,
'K_D': 0
} # Set PID values for lateral controller
vehicle_controller = VehiclePIDController(
self.vehicle, lon_param,
lat_param) # Calling vehicle controller class from controller.py
i = 0
for k in range(1, len(self.current_route)
): # Iterate through all the waypoints in the route
self.Distance(self.current_route[i][0], self.veh_pos.transform)
self.Waypoints()
rospy.Subscriber(
'/machine_learning/output', Int16, self.Detection
) # Subscribes to topic for Stop sign detection. Need to run carla_detect_objects.py script to obtain detection
while self.distance > 0.5: # Control the vehicle until the distance of the next waypoint and the vehicle is less than 0.5 m
self.Actor(
) # Call Actor function to update vehicle's location
control = vehicle_controller.run_step(
15, self.current_route[i]
[0]) # Feeds the controller the waypoints one by one
self.velocity = self.vehicle.get_velocity(
) # Get vehicle velocity
if self.detection == 11: # Stop sign detection(apply brakes). Our ML has a class ID of 11 for stop signs
print('Object detected, apply brakes')
msg = CarlaEgoVehicleControl(
) # Ego vehicle's control message
msg.throttle = 0
msg.steer = control.steer
msg.brake = 1
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
self.detection = None
elif len(self.current_route) - 5 <= k <= len(
self.current_route
): # If the ith waypoint is between the last waypoint minus five apply brakes
msg = CarlaEgoVehicleControl()
msg.throttle = 0
msg.steer = control.steer
msg.brake = 1
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
print('You arrived to your destination!!')
else: # If neither scenario happen, keep driving
msg = CarlaEgoVehicleControl()
msg.throttle = control.throttle
msg.steer = control.steer
msg.brake = control.brake
msg.hand_brake = control.hand_brake
msg.reverse = control.reverse
msg.gear = 1
msg.manual_gear_shift = control.manual_gear_shift
self.Publisher(msg)
rate.sleep()
self.Distance(
self.current_route[i][0], self.veh_pos.transform
) # Calculates the Euclidean distance between the vehicle and the next waypoint in every iteration
i += 1
def main():
"""
main function
"""
role_name = rospy.get_param("~role_name", "ego_vehicle")
code_control = CarlaEgoVehicleControl()
vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(role_name),
CarlaEgoVehicleControl,
queue_size=1)
rospy.init_node('carla_manual_control', anonymous=True)
# resolution should be similar to spawned camera with role-name 'view'
view_subscriber = rospy.Subscriber(
"/carla/{}/camera/rgb/view/image_color".format(role_name), Image,
on_view_view_image)
top_subscriber = rospy.Subscriber(
"/carla/{}/camera/rgb/top/image_color".format(role_name), Image,
on_view_top_image)
throttle_pub = rospy.Publisher('acceleratorServo', Float32, queue_size=1)
break_pub = rospy.Publisher('breakServo', Float32, queue_size=1)
steer_pub = rospy.Publisher('steeringServo', Float32, queue_size=1)
resolution = {"width": 1920, "height": 1080}
pygame.init()
pygame.font.init()
pygame.display.set_caption("CARLA ROS manual control")
world = None
skip_first_frame = True
prev_timestamp = 0
tot_target_reached = 0
num_min_waypoints = 21
try:
start_timestamp = 0
display = pygame.display.set_mode(
(resolution['width'], resolution['height']),
pygame.HWSURFACE | pygame.DOUBLEBUF)
hud = HUD(role_name, resolution['width'], resolution['height'])
world = World(role_name, hud)
controllerK = KeyboardControl(role_name, hud)
clock = pygame.time.Clock()
waypoints_file = WAYPOINTS_FILENAME
waypoints_filepath =\
os.path.join(os.path.dirname(os.path.realpath(__file__)),
WAYPOINTS_FILENAME)
waypoints_np = None
with open(waypoints_filepath) as waypoints_file_handle:
waypoints = list(
csv.reader(waypoints_file_handle,
delimiter=',',
quoting=csv.QUOTE_NONNUMERIC))
waypoints_np = np.array(waypoints)
wp_goal_index = 0
local_waypoints = None
path_validity = np.zeros((NUM_PATHS, 1), dtype=bool)
controller = resources.controller2d.Controller2D(waypoints)
bp = resources.behavioural_planner.BehaviouralPlanner(
BP_LOOKAHEAD_BASE)
lp = resources.local_planner.LocalPlanner(NUM_PATHS, PATH_OFFSET,
CIRCLE_OFFSETS, CIRCLE_RADII,
PATH_SELECT_WEIGHT, TIME_GAP,
A_MAX, SLOW_SPEED,
STOP_LINE_BUFFER)
clock = pygame.time.Clock()
frame = 0
current_timestamp = start_timestamp
reached_the_end = False
tx = 0
ty = 2.0
tz = 2.0
CameraFOV = 100
Cu = 1920 / 2
Cv = 1080 / 2
f = 1920 / (2 * np.tan(CameraFOV * np.pi / 360))
K = [[f, 0, Cu], [0, f, Cv], [0, 0, 1]]
K_np = np.asmatrix(K)
t = [[1, 0, 0, tx], [0, 1, 0, ty], [0, 0, 1, tz]]
t_np = np.asmatrix(t)
while not rospy.core.is_shutdown():
vehicle_control_publisher.publish(code_control)
frame = frame + 1
clock.tick_busy_loop(60)
if controllerK.parse_events(clock):
return
hud.tick(clock)
world.render(display)
pygame.display.flip()
##transform = world.role_name.get_transform()
##vel = world.role_name.get_velocity()
prev_timestamp = current_timestamp
current_timestamp = rospy.get_time()
##control = world.role_name.get_control()
try:
tf_listener = hud.tf_listener
(position, quaternion) = tf_listener.lookupTransform(
'/map', role_name, rospy.Time())
_, _, yaw = tf.transformations.euler_from_quaternion(
quaternion)
yaw = -math.degrees(yaw)
x = position[0]
y = -position[1]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
x = 0
y = 0
z = 0
yaw = 0
current_x = x
current_y = y
current_yaw = np.radians(yaw)
velocity = hud.vehicle_status.velocity
current_speed = velocity
open_loop_speed = lp._velocity_planner.get_open_loop_speed(
current_timestamp - prev_timestamp)
#open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp)
if frame % 5 == 0:
# try:
# cv2.imshow("view_image",view_bgr_image)
# cv2.imshow("top image",top_bgr_image)
# #cv2.imshow("top image",top_bgr_image)
#
#cv2.waitKey(1)
#except Exception as e:
# print(e)
#Lane detection
ego_state = [
current_x + 2, current_y, current_yaw, open_loop_speed
]
print(ego_state)
bp.set_lookahead(BP_LOOKAHEAD_BASE +
BP_LOOKAHEAD_TIME * open_loop_speed)
bp.transition_state(waypoints, waypoints, ego_state,
current_speed)
goal_state_set = lp.get_goal_state_set(bp._goal_index,
bp._goal_state,
waypoints, ego_state)
#print("goal_state_set",goal_state_set)
paths_vehicle_coordinate, path_validity = lp.plan_paths(
goal_state_set)
# print("Paths_local",paths_vehicle_coordinate)
paths = resources.local_planner.transform_paths(
paths_vehicle_coordinate, ego_state)
#print("Paths_global",paths)
best_index = lp._collision_checker.select_best_path_index(
paths, bp._goal_state_hyp)
################################################ Compute paths visualize#####################
#Whole Code
# try:
#
# paths_vehicle_coordinate_np = np.array(paths_vehicle_coordinate)
#
# Each_Path_length = len(paths_vehicle_coordinate_np[0][0])
#
# pixel_coordinate_paths = []
#
# for x in range(0,NUM_PATHS):
# for i in range(0,Each_Path_length):
# world_coordinate = [paths_vehicle_coordinate_np[x][1][i],1,paths_vehicle_coordinate_np[x][0][i],1]
# world_coordinate_np = np.asmatrix(world_coordinate)
# camera_coordinates = np.dot(np.dot(K_np,t),world_coordinate_np.T)
# pixel_coordinates = [[camera_coordinates[0] / camera_coordinates[2]],
# [camera_coordinates[1] / camera_coordinates[2]]
# ]
# pixel_coordinate_paths.append(pixel_coordinates)
# pixel_coordinate_paths_np = np.array(pixel_coordinate_paths)
# pixel_plot = pixel_coordinate_paths_np[:,:,0, 0, 0]
# pix_1 = pixel_plot[0:49]
# pix_2 = pixel_plot[49:98]
# pix_3 = pixel_plot[98:147]
# pix_4 = pixel_plot[147:196]
# pix_5 = pixel_plot[196:245]
# pix_6 = pixel_plot[245:294]
# pix_7 = pixel_plot[294:343]
#
# line_1 = np.array((pix_1.T[0],pix_1.T[1]))
# line_1 = pix_1.astype(int)
# line_1 = line_1.reshape((-1,1,2))
#
# line_2 = pix_2.astype(int)
# line_2 = line_2.reshape((-1,1,2))
#
# line_3 = pix_3.astype(int)
# line_3 = line_3.reshape((-1,1,2))
#line_4 = pix_4.astype(int)
#line_4 = line_4.reshape((-1,1,2))
#line_5 = pix_5.astype(int)
#line_5 = line_5.reshape((-1,1,2))
#line_6 = pix_6.astype(int)
#line_6 = line_6.reshape((-1,1,2))
#line_7 = pix_7.astype(int)
# line_7 = line_7.reshape((-1,1,2))
# on_view_image_global_np = np.array(on_view_image_global)
# view_bgr_image_np = np.array(on_view_image_global)
# line_array = [line_1,line_2,line_3,line_4,line_5,line_6,line_7]
# print(line_array)
# print("works 1")
# image = cv2.polylines(on_view_image_global_np,[line_1,line_2,line_3,line_4,line_5,line_6,line_7],
# isClosed=False,
# color=(255, 255, 255),
# thickness=10,
# lineType=cv2.LINE_AA)
# print("Works here")
# image = cv2.polylines(image,[line_array[best_index]],
# isClosed=False,
# color=(0, 255, 0),
# thickness=10,
# lineType=cv2.LINE_AA)
# cv2.imshow("front image",image[:,:,::-1])
# cv2.waitKey(1)
#
#except Exception as e:
# print(e)
try:
if best_index == None:
best_path = lp._prev_best_path
else:
best_path = paths[best_index]
lp._prev_best_path = best_path
except:
continue
desired_speed = bp._goal_state[2]
decelerate_to_stop = False
print("Desired_speed", desired_speed)
print("Current_speed", current_speed)
local_waypoints = lp._velocity_planner.compute_velocity_profile(
best_path, desired_speed, ego_state, current_speed)
print("local_waypoints", local_waypoints)
if local_waypoints != None:
wp_distance = [] # distance array
local_waypoints_np = np.array(local_waypoints)
for i in range(1, local_waypoints_np.shape[0]):
wp_distance.append(
np.sqrt((local_waypoints_np[i, 0] -
local_waypoints_np[i - 1, 0])**2 +
(local_waypoints_np[i, 1] -
local_waypoints_np[i - 1, 1])**2))
wp_distance.append(0)
wp_interp = []
for i in range(local_waypoints_np.shape[0] - 1):
wp_interp.append(list(local_waypoints_np[i]))
num_pts_to_interp = int(np.floor(wp_distance[i] /\
float(INTERP_DISTANCE_RES)) - 1)
wp_vector = local_waypoints_np[
i + 1] - local_waypoints_np[i]
wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2])
for j in range(num_pts_to_interp):
next_wp_vector = INTERP_DISTANCE_RES * float(
j + 1) * wp_uvector
wp_interp.append(
list(local_waypoints_np[i] + next_wp_vector))
wp_interp.append(list(local_waypoints_np[-1]))
controller.update_waypoints(wp_interp)
pass
if local_waypoints != None and local_waypoints != []:
controller.update_values(current_x, current_y, current_yaw,
current_speed, current_timestamp,
frame)
controller.update_controls()
cmd_throttle, cmd_steer, cmd_brake = controller.get_commands()
else:
cmd_throttle = 0.0
cmd_steer = 0.0
cmd_brake = 0.0
if skip_first_frame and frame == 0:
pass
elif local_waypoints == None:
pass
else:
## if i % 5 == 0:
wp_interp_np = np.array(wp_interp)
path_indices = np.floor(
np.linspace(0, wp_interp_np.shape[0] - 1,
INTERP_MAX_POINTS_PLOT))
#if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop:
# agent.reroute(spawn_points)
# tot_target_reached += 1
# world.hud.notification("The target has been reached " +
# str(tot_target_reached) + " times.", seconds=4.0)
#elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop:
# print("Target reached, mission accomplished...")
# break
#speed_limit = world.role_name.get_speed_limit()
#agent.get_local_planner().set_speed(speed_limit)
#cmd_throttle = 0.4
#cmd_steer = 0
#cmd_brake = 0
dist_to_last_waypoint = np.linalg.norm(
np.array([
waypoints[-1][0] - current_x, waypoints[-1][1] - current_y
]))
if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT:
reached_the_end = True
if reached_the_end:
cmd_steer = np.fmax(np.fmin(0, 1.0), -1.0)
cmd_throttle = np.fmax(np.fmin(0, 1.0), 0)
cmd_brake = np.fmax(np.fmin(0.1, 1.0), 0)
break
#cmd_steer = np.fmax(np.fmin(cmd_steer, 1.0), -1.0)
kmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0)
#cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0)
print("Throttle before", cmd_throttle)
print("Throttle after", kmd_throttle)
code_control.throttle = cmd_throttle
code_control.steer = cmd_steer
code_control.brake = cmd_brake
code_control.gear = 3
vehicle_control_publisher.publish(code_control)
print("Code control", code_control)
throttle_angle = code_control.throttle * 135 #Kind of makes sense but the maximum is 130 degrees lets say random guess :) #Ill have to change the zeros
break_angle = code_control.brake * 135
steer_angle = (
code_control.steer * -135
) + 95 #here we state that 0 is the 95 degrees so anything below tends towards 0 and viceversa
print("steer", cmd_steer)
throttle_pub.publish(throttle_angle)
break_pub.publish(break_angle)
steer_pub.publish(steer_angle)
#world.role_name.apply_control(carla.VehicleControl(throttle=cmd_throttle,
# steer=cmd_steer,
# brake=cmd_brake))
if reached_the_end:
print("Reached the end of path. Writing to controller_output...")
else:
print("Exceeded assessment time. Writing to controller_output...")
finally:
if world is not None:
world.destroy()
pygame.quit()
