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
self.pub.publish(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
try:
self.pub.publish(control)
except rospy.ROSException as e:
if not rospy.is_shutdown():
rospy.logwarn("Error while publishing control: {}".format(e))
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 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.throttle = cmd_msg.speed
msg.steer = cmd_msg.steering_angle
ack.pub_callback(msg)
rate.sleep()
def __init__(self, role_name):
self.role_name = role_name
self.vehicle_control_manual_override_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
Bool,
queue_size=1,
latch=True,
)
self.vehicle_control_manual_override = True
self.auto_pilot_enable_publisher = rospy.Publisher(
"/carla/{}/enable_autopilot".format(self.role_name),
Bool,
queue_size=1)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
CarlaEgoVehicleControl,
queue_size=1,
)
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self.set_autopilot(self._autopilot_enabled)
self._steer_cache = 0.0
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def run_step(self, target_speed): # pylint: disable=no-self-use,unused-argument
"""
Execute one step of navigation.
:return: control
"""
control = CarlaEgoVehicleControl()
return control
def __init__(self, role_name, hud):
self.role_name = role_name
self.hud = hud
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self._steer_cache = 0.0
self.vehicle_control_manual_override_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
Bool,
queue_size=1,
latch=True)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = rospy.Publisher(
"/carla/{}/enable_autopilot".format(self.role_name),
Bool,
queue_size=1)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
CarlaEgoVehicleControl,
queue_size=1)
self.carla_status_subscriber = rospy.Subscriber(
"/carla/status", CarlaStatus, self._on_new_carla_frame)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def __init__(self, role_name, hud):
self.role_name = role_name
self.hud = hud
self.vehicle_control_manual_override_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
Bool,
queue_size=1,
latch=True)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = rospy.Publisher(
"/carla/{}/enable_autopilot".format(self.role_name),
Bool,
queue_size=1)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
CarlaEgoVehicleControl,
queue_size=1)
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self.set_autopilot(self._autopilot_enabled)
self._steer_cache = 0.0
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
self.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
def main():
global curr_vel
client = carla.Client('localhost', 2000)
client.set_timeout(5.0)
world = client.get_world()
m = world.get_map()
debug = world.debug
ego_control = CarlaEgoVehicleControl()
ego_status = CarlaEgoVehicleStatus()
rospy.init_node('driver', anonymous=True)
pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd',
CarlaEgoVehicleControl,
queue_size=1)
rospy.Subscriber('/carla/ego_vehicle/vehicle_status',
CarlaEgoVehicleStatus,
status_callback,
queue_size=1)
rospy.Subscriber('/carla/ego_vehicle/odometry',
Odometry,
pos_callback,
queue_size=1)
waypoints = making_traj(m, debug)
wp_xs = waypoints[0]
wp_ys = waypoints[1]
wp_yaws = waypoints[2]
rate = rospy.Rate(10)
while not rospy.is_shutdown():
# generate acceleration, and steering
speed_error = target_speed - curr_vel
acc = Kp * speed_error
# caution y and yaw sign
steer = stanley_control(curr_pos["DX"], -curr_pos["DY"], -curr_yaw,
curr_vel, wp_xs, wp_ys, wp_yaws)
print("x: {}, y: {}, yaw: {}".format(curr_pos["DX"], -curr_pos["DY"],
-np.degrees(curr_yaw)))
# Publishing Topic
ego_control.throttle = acc
ego_control.steer = steer
pub.publish(ego_control)
print("steering: {:.3f}".format(steer))
print('\n')
rate.sleep()
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 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 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):
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 __init__(self, carla_actor, parent, communication,
vehicle_control_applied_callback):
"""
Constructor
:param carla_actor: carla actor object
:type carla_actor: carla.Actor
:param parent: the parent of this
:type parent: carla_icv_bridge.Parent
:param communication: communication-handle
:type communication: carla_icv_bridge.communication
"""
super(EgoVehicle,
self).__init__(carla_actor=carla_actor,
parent=parent,
communication=communication,
prefix=carla_actor.attributes.get('role_name'))
self.vehicle_info_published = False
self.vehicle_control_override = False
self._vehicle_control_applied_callback = vehicle_control_applied_callback
self.sub1 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd")
self.sub2 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_cmd_manual")
self.sub3 = Subscriber(self.get_topic_prefix() +
"/vehicle_control_manual_override")
self.sub4 = Subscriber(self.get_topic_prefix() + "/enable_autopilot")
self.sub5 = Subscriber(self.get_topic_prefix() + "/twist_cmd")
self.Sec_loop = 0.02
self.control_subscriber = CarlaEgoVehicleControl()
self.manual_control_subscriber = CarlaEgoVehicleControl()
self.control_override_subscriber = Bool()
self.enable_autopilot_subscriber = Bool()
self.twist_control_subscriber = Twist()
self.pub1 = Publisher(self.get_topic_prefix() + "/vehicle_info")
self.pub2 = Publisher(self.get_topic_prefix() + "/vehicle_status")
self.update_command_thread = Thread(
target=self._update_commands_thread)
self.update_command_thread.start()
def dummy_agent():
rospy.init_node("dummy_agent")
rate = rospy.Rate(20) # 20hz
publisher = rospy.Publisher("/carla/hero/vehicle_control_cmd",
CarlaEgoVehicleControl,
queue_size=1)
ready = rospy.Publisher("/carla/hero/status",
Bool,
latch=True,
queue_size=1)
ready.publish(True)
while not rospy.is_shutdown():
control_msg = CarlaEgoVehicleControl()
control_msg.header.stamp = rospy.Time.now()
control_msg.throttle = 1.0
publisher.publish(control_msg)
rate.sleep()
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 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 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 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 __init__(self, role_name, hud, node):
self.role_name = role_name
self.hud = hud
self.node = node
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self._steer_cache = 0.0
if ROS_VERSION == 1:
self.callback_group = None
elif ROS_VERSION == 2:
self.callback_group = ReentrantCallbackGroup()
fast_qos = QoSProfile(depth=10)
fast_latched_qos = QoSProfile(depth=10, durability=latch_on) # imported from ros_compat.
self.vehicle_control_manual_override_publisher = \
self.node.new_publisher(Bool,
"/carla/{}/vehicle_control_manual_override".format(
self.role_name),
qos_profile=fast_latched_qos, callback_group=self.callback_group)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = \
self.node.new_publisher(Bool,
"/carla/{}/enable_autopilot".format(self.role_name),
qos_profile=fast_qos, callback_group=self.callback_group)
self.vehicle_control_publisher = \
self.node.new_publisher(CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
qos_profile=fast_qos, callback_group=self.callback_group)
self.carla_status_subscriber = self.node.create_subscriber(CarlaStatus, "/carla/status",
self._on_new_carla_frame,
callback_group=self.callback_group)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def __init__(self, role_name, hud):
self.role_name = role_name
self.hud = hud
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self._deep = CarlaDeepData()
self._steer_cache = 0.0
self.vehicle_deep_pb = rospy.Publisher("/carla/vehicle_deep_data",
CarlaDeepData,
queue_size=1)
self.vehicle_deep_sub_status = rospy.Subscriber(
"/carla/{}/vehicle_status".format(self.role_name),
CarlaEgoVehicleStatus, self.update_deep)
self.vehicle_deep_sub_image = rospy.Subscriber(
"/carla/ego_vehicle/camera/rgb/front/image_color", Image,
self.update_deep_image)
self.vehicle_control_manual_override_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
Bool,
queue_size=1,
latch=True)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = rospy.Publisher(
"/carla/{}/enable_autopilot".format(self.role_name),
Bool,
queue_size=1)
self.vehicle_control_publisher = rospy.Publisher(
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
CarlaEgoVehicleControl,
queue_size=1)
self.carla_status_subscriber = rospy.Subscriber(
"/carla/status", CarlaStatus, self._on_new_carla_frame)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
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 servo_fine():
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)
rospy.init_node('servo_controller', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
code_control = CarlaEgoVehicleControl()
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.publish(throttle_angle)
break_pub.publish(break_angle)
steer_pub.publish(steer_angle)
rate.sleep()
def __init__(self, role_name, host, port):
self.role_name = role_name
client = carla.Client(host, port)
self.world = client.get_world()
self.map = self.world.get_map()
self.vehicle_dyn = VehicleDynamics()
self.state = None
self.input = [0, 0]
self.vehicle = None
self.speed_control = PID(Kp=1.5,
Ki=1.5,
Kd=0.,
sample_time=0.05,
output_limits=(0., 1.))
self.vehicle_control_cmd = CarlaEgoVehicleControl(throttle=0.)
self.find_ego_vehicle()
self.init_state()
subControl = rospy.Subscriber('/carla/%s/vehicle_control'%role_name, CarlaEgoVehicleControl, self.controlCallback)
# subControl = rospy.Subscriber('/carla/%s/vehicle_control'%role_name, CarlaEgoVehicleControl, self.controlCallback)
subAckermann = rospy.Subscriber('/carla/%s/ackermann_control'%role_name, AckermannDrive, self.ackermannCallback)
def __init__(self, role_name, hud, node):
self.role_name = role_name
self.hud = hud
self.node = node
self._autopilot_enabled = False
self._control = CarlaEgoVehicleControl()
self._steer_cache = 0.0
fast_qos = QoSProfile(depth=10)
fast_latched_qos = QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL)
self.vehicle_control_manual_override_publisher = self.node.new_publisher(
Bool,
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
qos_profile=fast_latched_qos)
self.vehicle_control_manual_override = False
self.auto_pilot_enable_publisher = self.node.new_publisher(
Bool,
"/carla/{}/enable_autopilot".format(self.role_name),
qos_profile=fast_qos)
self.vehicle_control_publisher = self.node.new_publisher(
CarlaEgoVehicleControl,
"/carla/{}/vehicle_control_cmd_manual".format(self.role_name),
qos_profile=fast_qos)
self.carla_status_subscriber = self.node.new_subscription(
CarlaStatus,
"/carla/status",
self._on_new_carla_frame,
qos_profile=10)
self.set_autopilot(self._autopilot_enabled)
self.set_vehicle_control_manual_override(
self.vehicle_control_manual_override) # disable manual override
def throttle_control_callback(cmd):
global throttle
throttle = cmd.data
if __name__ == '__main__':
rospy.Subscriber('/carla/{}/cv_steer'.format(role_name), Float32,
cv_steer_callback)
rospy.Subscriber('/carla/{}/throttle_control'.format(role_name), Float32,
throttle_control_callback)
throttle_value = 0.0
if len(sys.argv) > 1:
throttle_value = float(sys.argv[1])
#TODO: declare to publish CarlaEgoVehicleControl message to 'cmd_topic_name' topic. Set queue_size to 1.
pub_cmd = rospy.Publisher(cmd_topic_name,
CarlaEgoVehicleControl,
queue_size=1)
rospy.init_node('driver_byl24', anonymous=False)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
#TODO: publish CarlaEgoVehicleControl message
rospy.loginfo('name: Brandon Liu, throttle: {}, steer: {}'.format(
throttle, cv_steer))
pub_cmd.publish(
CarlaEgoVehicleControl(throttle=throttle, steer=cv_steer))
rate.sleep()
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()
import glob
from geometry_msgs.msg import Twist
sys.path.append(
'home/kankan/carla/Dist/CARLA_Shipping_0.9.7-2-g73f91e0b/LinuxNoEditor/PythonAPI/carla/dist/carla-0.9.7-py3.6-linux-x86_64.egg'
)
import carla
from carla_msgs.msg import CarlaEgoVehicleControl
import std_msgs
from pynput.keyboard import Key, Listener
speed = 0.0
threshold = 0.5
steer = 0.0
vel_msg = CarlaEgoVehicleControl()
velocity_publisher = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd',
CarlaEgoVehicleControl,
queue_size=10)
class KeyboardPublisher:
def on_press(self, key):
global steer
#print(key) #<-- this prints out the key press and then crashes
try:
if key.char == 'd' and steer < 1:
steer = steer + 0.2
if key.char == 'a' and steer > -1:
steer = steer - 0.2
except AttributeError:
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
draw_lines(temp, R_avg)
result = weighted_img(temp, src) #overlap
cv2.imshow('result', result) #print
cv2.waitKey(2)
rospy.init_node('my_node')
rospy.Subscriber('/carla/ego_vehicle/rgb_front/image',
Image,
callback,
queue_size=1)
pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd_manual',
CarlaEgoVehicleControl,
queue_size=1)
pub_msg = CarlaEgoVehicleControl()
#rate = rospy.Rate()
while not rospy.is_shutdown():
if right == 0:
pub_msg.throttle = 0.5
pub_msg.steer = steer
pub.publish(pub_msg)
#rate.sleep()
pub_msg.throttle = 0.3
pub_msg.steer = -steer
pub.publish(pub_msg)
#rate.sleep()
pub_msg.throttle = 1
pub_msg.steer = 0
pub.publish(pub_msg)