def run_step(self, target_speed):
"""
Execute one step of local planning which involves running the longitudinal
and lateral PID controllers to follow the waypoints trajectory.
"""
if not self._waypoints_queue:
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
control.manual_gear_shift = False
return control
# Buffering the waypoints
if not self._waypoint_buffer:
for i in range(self._buffer_size):
if self._waypoints_queue:
self._waypoint_buffer.append(
self._waypoints_queue.popleft())
else:
break
# current vehicle waypoint
self._current_waypoint = self.get_waypoint(self._current_pose.position)
# target waypoint
target_route_point = self._waypoint_buffer[0]
# for us redlight-detection
self.target_waypoint = self.get_waypoint(target_route_point.position)
target_point = PointStamped()
target_point.header.frame_id = "map"
target_point.point.x = target_route_point.position.x
target_point.point.y = target_route_point.position.y
target_point.point.z = target_route_point.position.z
self._target_point_publisher.publish(target_point)
# move using PID controllers
control = self._vehicle_controller.run_step(target_speed,
self._current_speed,
self._current_pose,
target_route_point)
# purge the queue of obsolete waypoints
max_index = -1
sampling_radius = target_speed * 1 / 3.6 # 1 seconds horizon
min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE
for i, route_point in enumerate(self._waypoint_buffer):
if distance_vehicle(route_point,
self._current_pose.position) < min_distance:
max_index = i
if max_index >= 0:
for i in range(max_index + 1):
self._waypoint_buffer.popleft()
return control
def run_step(self):
"""
Execute one step of navigation.
"""
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 0.0
control.hand_brake = False
if not self._agent:
rospy.loginfo("Waiting for ego vehicle...")
return control
if not self._route_assigned and self._global_plan:
rospy.loginfo("Assigning plan...")
self._agent._local_planner.set_global_plan( # pylint: disable=protected-access
self._global_plan.poses)
self._route_assigned = True
else:
control, finished = self._agent.run_step(self._target_speed)
if finished:
self._global_plan = None
self._route_assigned = False
return control
def twist_received(self, twist):
"""
receive twist and convert to carla vehicle control
"""
control = CarlaEgoVehicleControl()
if twist == Twist():
#stop
control.throttle = 0.
control.brake = 1.
control.steer = 0.
else:
if twist.linear.x > 0:
control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \
TwistToVehicleControl.MAX_LON_ACCELERATION
else:
control.reverse = True
control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \
-TwistToVehicleControl.MAX_LON_ACCELERATION
if twist.angular.z > 0:
control.steer = -min(self.max_steering_angle, twist.angular.z)/ \
self.max_steering_angle
else:
control.steer = -max(-self.max_steering_angle, twist.angular.z)/ \
self.max_steering_angle
#invert steer for reverse case
if control.reverse:
control.steer = -control.steer
self.pub.publish(control)
def run_step(self, target_speed, current_speed, current_pose, waypoint):
"""
Execute one step of control invoking both lateral and longitudinal
PID controllers to reach a target waypoint at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:return: distance (in meters) to the waypoint
"""
current_time = rospy.get_time()
dt = current_time - self._last_control_time
if dt == 0.0:
dt = 0.000001
control = CarlaEgoVehicleControl()
throttle = self._lon_controller.run_step(target_speed, current_speed,
dt)
steering = self._lat_controller.run_step(current_pose, waypoint, dt)
self._last_control_time = current_time
control.steer = steering
control.throttle = throttle
control.brake = 0.0
control.hand_brake = False
control.manual_gear_shift = False
return control
def tick(self):
target_speed = self._get_target_speed()
speed_err = target_speed - self.speed
cte = self._calc_lateral_error() # cross-track error
max_delta = 100
steer = clamp(-1, self.steer_pid.update(cte), 1)
# steer = clamp(self.prev_steer - max_delta, steer, self.prev_steer + max_delta)
self.prev_steer = steer
brake = 0
throttle = clamp(-1, self.throttle_pid.update(speed_err), 1)
if throttle < 0:
throttle = 0
brake = -throttle
# public control command
control = CarlaEgoVehicleControl()
control.throttle = throttle
control.steer = steer
control.brake = brake
control.hand_brake = False
self._vehicle_control_publisher.publish(control)
# publish debug info
debug_info_msg = msg.ControllerDebugInfo()
debug_info_msg.cross_track_error = cte
debug_info_msg.speed_error = speed_err
debug_info_msg.target_speed = target_speed
self._debug_info_publisher.publish(debug_info_msg)
def twist_received(self, twist):
"""
receive twist and convert to carla vehicle control
"""
control = CarlaEgoVehicleControl()
if twist == Twist():
# stop
control.throttle = 0.
control.brake = 1.
control.steer = 0.
else:
if twist.linear.x > 0:
control.throttle = min(
TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x
) / TwistToVehicleControl.MAX_LON_ACCELERATION
else:
control.reverse = True
control.throttle = max(
-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x
) / -TwistToVehicleControl.MAX_LON_ACCELERATION
if twist.angular.z > 0:
control.steer = -min(self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
else:
control.steer = -max(-self.max_steering_angle, twist.angular.z) / \
self.max_steering_angle
try:
self.pub.publish(control)
except rospy.ROSException as e:
if not rospy.is_shutdown():
rospy.logwarn("Error while publishing control: {}".format(e))
def emergency_stop(self):
control_msg = CarlaEgoVehicleControl()
control_msg.steer = 0.0
control_msg.throttle = 0.0
control_msg.brake = 1.0
control_msg.hand_brake = False
control_msg.manual_gear_shift = False
self._control_cmd_publisher.publish(control_msg)
def steer_cmd_pub(self, steer_cmd, brake_cmd, thorttle_cmd):
msg = CarlaEgoVehicleControl()
msg.header.stamp = rospy.Time.now()
msg.steer = steer_cmd
msg.brake = brake_cmd
msg.throttle = thorttle_cmd
self.pub_steer_cmd.publish(msg)
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 emergency_stop(self): # pylint: disable=no-self-use
"""
Send an emergency stop command to the vehicle
:return:
"""
control = CarlaEgoVehicleControl()
control.steer = 0.0
control.throttle = 0.0
control.brake = 1.0
control.hand_brake = False
return control
def run_step(self, target_speed, current_speed, current_pose, waypoint):
"""
Execute one step of control invoking both lateral and longitudinal
PID controllers to reach a target waypoint at a given target_speed.
:param target_speed: desired vehicle speed
:param waypoint: target location encoded as a waypoint
:return: control signal (throttle and steering)
"""
control = CarlaEgoVehicleControl()
throttle = self._lon_controller.run_step(target_speed, current_speed)
steering = self._lat_controller.run_step(current_pose, waypoint)
control.steer = -steering
control.throttle = throttle
control.brake = 0.0
control.hand_brake = False
control.manual_gear_shift = False
return control
def main():
ack = Ani()
rospy.init_node('Carla_control')
rate = rospy.Rate(10)
while not rospy.is_shutdown():
cmd_msg = AckermannDrive()
msg = CarlaEgoVehicleControl()
cmd_msg.speed = ack.speed
cmd_msg.steering_angle = ack.steering_angle
msg.steer = cmd_msg.steering_angle
msg.throttle = cmd_msg.speed
if ack.brake == 1:
if cmd_msg.speed < 0:
msg.reverse = 15
if ack.brake == -1:
msg.brake = True
ack.pub_callback(msg)
rate.sleep()
def step(self, action):
#print("step")
# self.ego_vehicle.apply_control(carla.VehicleControl(throttle = action[0], steer = action[1], brake = action[2]))
p = CarlaEgoVehicleControl()
p.header.stamp = rospy.Time.now()
p.throttle = action[0]
p.steer = action[1]
p.brake = 0
print(action[0])
print(action[1])
print(action[2])
act_pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, queue_size = 100)
act_pub.publish(p)
rospy.sleep(1)
is_done = False
if self.episode_start + SECONDS_PER_EPISODE < time.time():
is_done = True
reward = None
return OBS.get_observation(), reward, is_done, None
# def close(self):
# pass
# try:
# sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
# sys.version_info.major,
# sys.version_info.minor,
# 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
# except IndexError
# pass
# register(
# id='CarlaEnv-v0',
# entry_point=':custom_environments.envs:CarlaEnv'
# )
def main():
"""
main function
"""
rospy.init_node('carla_manual_control', anonymous=True)
role_name = rospy.get_param("~role_name", "ego_vehicle")
vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(role_name),
CarlaEgoVehicleControl,
queue_size=10)
# resolution should be similar to spawned camera with role-name 'view'
resolution = {"width": 800, "height": 600}
pygame.init()
pygame.font.init()
pygame.display.set_caption("CARLA ROS manual control")
world = None
tf_listener = tf.TransformListener()
code_control = CarlaEgoVehicleControl()
vehicle_status = CarlaEgoVehicleStatus()
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)
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
while not rospy.core.is_shutdown():
frame = frame + 1
clock.tick_busy_loop(60)
world.render(display)
if controllerK.parse_events(clock):
return
hud.tick(clock)
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:
(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)
current_yaw = -yaw
velocity = hud.vehicle_status.velocity
current_speed = velocity
print("current_speed", current_speed)
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)
print("open_loop_speed", open_loop_speed)
if frame % 5 == 0:
#Lane detection
ego_state = [
current_x, current_y, current_yaw, open_loop_speed
]
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)
paths, path_validity = lp.plan_paths(goal_state_set)
paths = resources.local_planner.transform_paths(
paths, ego_state)
best_index = lp._collision_checker.select_best_path_index(
paths, bp._goal_state_hyp)
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
local_waypoints = lp._velocity_planner.compute_velocity_profile(
best_path, desired_speed, ego_state, current_speed)
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)
cmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0)
cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0)
#print("throttle",cmd_throttle)
code_control.throttle = cmd_throttle
code_control.steer = cmd_steer
code_control.brake = cmd_brake
#servo_fine()
#servo_fine()
#throttle_angle = code_control.throttle * 270
#steer_angle = code_control.steer * 270
#break_angle = code_control.brake * 270
#rospy.loginfo(throttle_angle)
#rospy.loginfo(break_angle)
#rospy.loginfo(steer_angle)
#throttle_pub = rospy.Publisher('acceleratorServo', Float32, queue_size=10)
#break_pub = rospy.Publisher('breakServo', Float32, queue_size=10)
#steer_pub = rospy.Publisher('steeringServo', Float32, queue_size=10)
#throttle_pub.publish(throttle_angle)
#break_pub.publish(break_angle)
#steer_pub.publish(steer_angle)
#print(code_control.throttle)
#print("Ros_throttle",code_control.throttle)
vehicle_control_publisher.publish(code_control)
#print(code_control)
#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()
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()
