def __init__(self, role_name, width, height, node):
self.role_name = role_name
self.dim = (width, height)
self.node = node
font = pygame.font.Font(pygame.font.get_default_font(), 20)
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
default_font = 'ubuntumono'
mono = default_font if default_font in fonts else fonts[0]
mono = pygame.font.match_font(mono)
self._font_mono = pygame.font.Font(mono, 14)
self._notifications = FadingText(font, (width, 40), (0, height - 40))
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
self._show_info = True
self._info_text = []
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_status_subscriber = node.new_subscription(
CarlaEgoVehicleStatus,
"/carla/{}/vehicle_status".format(self.role_name),
self.vehicle_status_updated,
qos_profile=10)
self.vehicle_info = CarlaEgoVehicleInfo()
self.vehicle_info_subscriber = node.new_subscription(
CarlaEgoVehicleInfo,
"/carla/{}/vehicle_info".format(self.role_name),
self.vehicle_info_updated,
qos_profile=QoSProfile(
depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL))
self.x, self.y, self.z = 0, 0, 0
self.yaw = 0
self.latitude = 0
self.longitude = 0
self.manual_control = False
self.gnss_subscriber = node.new_subscription(NavSatFix,
"/carla/{}/gnss".format(
self.role_name),
self.gnss_updated,
qos_profile=10)
self.odometry_subscriber = node.new_subscription(
Odometry,
"/carla/{}/odometry".format(self.role_name),
self.odometry_updated,
qos_profile=10)
self.manual_control_subscriber = node.new_subscription(
Bool,
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
self.manual_control_override_updated,
qos_profile=10)
self.carla_status = CarlaStatus()
self.status_subscriber = node.new_subscription(
CarlaStatus,
"/carla/status",
self.carla_status_updated,
qos_profile=10)
def __init__(self, role_name, width, height):
self.role_name = role_name
self.dim = (width, height)
font = pygame.font.Font(pygame.font.get_default_font(), 20)
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
default_font = 'ubuntumono'
mono = default_font if default_font in fonts else fonts[0]
mono = pygame.font.match_font(mono)
self._font_mono = pygame.font.Font(mono, 14)
self._notifications = FadingText(font, (width, 40), (0, height - 40))
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
self._show_info = True
self._info_text = []
self.vehicle_status = CarlaEgoVehicleStatus()
self.tf_listener = tf.TransformListener()
self.vehicle_status_subscriber = rospy.Subscriber(
"/carla/{}/vehicle_status".format(self.role_name),
CarlaEgoVehicleStatus, self.vehicle_status_updated)
self.vehicle_info = CarlaEgoVehicleInfo()
self.vehicle_info_subscriber = rospy.Subscriber(
"/carla/{}/vehicle_info".format(self.role_name),
CarlaEgoVehicleInfo, self.vehicle_info_updated)
self.latitude = 0
self.longitude = 0
self.manual_control = False
self.gnss_subscriber = rospy.Subscriber(
"/carla/{}/gnss/gnss1/fix".format(self.role_name), NavSatFix,
self.gnss_updated)
self.manual_control_subscriber = rospy.Subscriber(
"/carla/{}/vehicle_control_manual_override".format(self.role_name),
Bool, self.manual_control_override_updated)
self.carla_status = CarlaStatus()
self.status_subscriber = rospy.Subscriber("/carla/status", CarlaStatus,
self.carla_status_updated)
def publish_can(self, sensor_id, data):
"""
publish can data
"""
if not self.vehicle_info_publisher:
self.vehicle_info_publisher = rospy.Publisher(
'/carla/ego_vehicle/vehicle_info',
CarlaEgoVehicleInfo,
queue_size=1,
latch=True)
info_msg = CarlaEgoVehicleInfo()
for wheel in data['wheels']:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel['tire_friction']
wheel_info.damping_rate = wheel['damping_rate']
wheel_info.steer_angle = wheel['steer_angle']
wheel_info.disable_steering = wheel['disable_steering']
info_msg.wheels.append(wheel_info)
info_msg.max_rpm = data['max_rpm']
info_msg.moi = data['moi']
info_msg.damping_rate_full_throttle = data[
'damping_rate_full_throttle']
info_msg.damping_rate_zero_throttle_clutch_engaged
info_msg.damping_rate_zero_throttle_clutch_disengaged = data[
'damping_rate_zero_throttle_clutch_disengaged']
info_msg.use_gear_autobox = data['use_gear_autobox']
info_msg.clutch_strength = data['clutch_strength']
info_msg.mass = data['mass']
info_msg.drag_coefficient = data['drag_coefficient']
info_msg.center_of_mass.x = data['center_of_mass']['x']
info_msg.center_of_mass.y = data['center_of_mass']['y']
info_msg.center_of_mass.z = data['center_of_mass']['z']
self.vehicle_info_publisher.publish(info_msg)
msg = CarlaEgoVehicleStatus()
msg.header = self.get_header()
msg.velocity = data['speed']
self.speed = data['speed']
#todo msg.acceleration
msg.control.throttle = self.current_control.throttle
msg.control.steer = self.current_control.steer
msg.control.brake = self.current_control.brake
msg.control.hand_brake = self.current_control.hand_brake
msg.control.reverse = self.current_control.reverse
msg.control.gear = self.current_control.gear
msg.control.manual_gear_shift = self.current_control.manual_gear_shift
self.vehicle_status_publisher.publish(msg)
def __init__(self):
"""
Constructor
"""
# Socket handling
self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
self.s.settimeout(1000) # 1000 seconds
self.socket_ip = rospy.get_param('~socket_ip', '127.0.0.1')
self.socket_port = rospy.get_param('~socket_port', '8080')
self.ros_timestamp = 0
self.last_timestamp = 0
self.ce = CarlaEnabled()
self.ce.carla_enabled = False
self.re = RobotEnabled()
self.re.robot_enabled = False
self.ego_vehicle_id = -1
self.throttle_cmd = 0
self.brake_cmd = 0
self.steering_cmd = 0
self.current_veh_status = VehicleStatus()
self.current_ego_veh_status = CarlaEgoVehicleStatus()
self.current_pose = PoseStamped()
self.current_external_objects = ExternalObjectList()
self.current_twist = TwistStamped()
# init subscribers
self.robot_status_sub = rospy.Subscriber(
"controller/robot_status", RobotEnabled, self.robot_status_update)
self.veh_cmd_sub = rospy.Subscriber(
"/carla/ego_vehicle/vehicle_control_cmd", CarlaEgoVehicleControl, self.veh_control_cmd_update)
# # subscribe to a topic using rospy.Subscriber class
# UNCOMMENT line below for constant listening
# sub=rospy.Subscriber('CarlaEgoVehicleControl_Topic', CarlaEgoVehicleControl, receive_data_from_CARMA_callback)
# #publish messages to a topic using rospy.Publisher class
self.sim_time_pub = rospy.Publisher(
'/clock', Clock, queue_size=10)
self.ce_pub = rospy.Publisher(
'carla_enabled', CarlaEnabled, queue_size=1, latch=True)
self.vs_pub = rospy.Publisher(
'vehicle_status', VehicleStatus, queue_size=1)
self.ego_vs_pub = rospy.Publisher(
'/carla/ego_vehicle/vehicle_status', CarlaEgoVehicleStatus, queue_size=1)
self.pose_pub = rospy.Publisher(
'/localization/current_pose', PoseStamped, queue_size=1)
self.eol_pub = rospy.Publisher(
'/environment/external_objects', ExternalObjectList, queue_size=1)
self.current_twist_pub = rospy.Publisher(
'vehicle/twist', TwistStamped, queue_size=10)
def __init__(self, role_name, width, height, node):
self.role_name = role_name
self.dim = (width, height)
self.node = node
font = pygame.font.Font(pygame.font.get_default_font(), 20)
fonts = [x for x in pygame.font.get_fonts() if 'mono' in x]
default_font = 'ubuntumono'
mono = default_font if default_font in fonts else fonts[0]
mono = pygame.font.match_font(mono)
self._font_mono = pygame.font.Font(mono, 14)
self._notifications = FadingText(font, (width, 40), (0, height - 40))
self.help = HelpText(pygame.font.Font(mono, 24), width, height)
self._show_info = True
self._info_text = []
self.vehicle_status = CarlaEgoVehicleStatus()
if ROS_VERSION == 1:
self.tf_listener = tf.TransformListener()
self.callback_group = None
elif ROS_VERSION == 2:
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, node=self.node)
self.callback_group = ReentrantCallbackGroup()
self.vehicle_status_subscriber = node.create_subscriber(
CarlaEgoVehicleStatus, "/carla/{}/vehicle_status".format(self.role_name),
self.vehicle_status_updated, callback_group=self.callback_group)
self.vehicle_info = CarlaEgoVehicleInfo()
self.vehicle_info_subscriber = node.create_subscriber(
CarlaEgoVehicleInfo, "/carla/{}/vehicle_info".format(self.role_name),
self.vehicle_info_updated, callback_group=self.callback_group, qos_profile=QoSProfile(depth=10, durability=latch_on))
self.latitude = 0
self.longitude = 0
self.manual_control = False
self.gnss_subscriber = node.create_subscriber(
NavSatFix, "/carla/{}/gnss".format(self.role_name), self.gnss_updated,
callback_group=self.callback_group)
self.manual_control_subscriber = node.create_subscriber(
Bool, "/carla/{}/vehicle_control_manual_override".format(self.role_name),
self.manual_control_override_updated, callback_group=self.callback_group)
self.carla_status = CarlaStatus()
self.status_subscriber = node.create_subscriber(CarlaStatus, "/carla/status",
self.carla_status_updated,
callback_group=self.callback_group)
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 __init__(self):
self.loop_rate = rospy.Rate(10)
self.role_name = rospy.get_param("~role_name", 'ego_vehicle')
# ==========================================
# -- Subscriber ----------------------------
# ==========================================
# velocity commands from move_base
self.cmd_vel = Twist()
self.cmd_vel_subscriber = rospy.Subscriber("/cmd_vel", Twist,
self.cmd_vel_updated)
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_status_subscriber = rospy.Subscriber(
"carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus,
self.vehicle_status_updated)
# ==========================================
# -- Publisher ----------------------------
# ==========================================
# ackermann drive message for the ackermann control
self.ackermann_pub = rospy.Publisher("scoomatic/ackermann_cmd",
AckermannDrive,
queue_size=50)
self.vehicle_status_pub = rospy.Publisher("scoomatic/vehicle_status",
CarlaEgoVehicleStatus,
queue_size=50)
path = os.path.abspath(
os.path.join(os.path.dirname(__file__), "..", "..", "config",
"pid_params.yaml"))
os.system(
"rosrun dynamic_reconfigure dynparam load /carla/ego_vehicle/ackermann_control {path}"
.format(path=path))
def send_vehicle_msgs(self):
"""
send messages related to vehicle status
:return:
"""
vehicle_status = CarlaEgoVehicleStatus(
header=self.get_msg_header("map"))
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration.linear = self.get_current_ros_accel(
).linear
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.vehicle_status_publisher.publish(vehicle_status)
# only send vehicle once (in latched-mode)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.max_steer_angle = math.radians(
wheel.max_steer_angle)
wheel_info.radius = wheel.radius
wheel_info.max_brake_torque = wheel.max_brake_torque
wheel_info.max_handbrake_torque = wheel.max_handbrake_torque
wheel_info.position.x = (wheel.position.x/100.0) - \
self.carla_actor.get_transform().location.x
wheel_info.position.y = -(
(wheel.position.y / 100.0) -
self.carla_actor.get_transform().location.y)
wheel_info.position.z = (wheel.position.z/100.0) - \
self.carla_actor.get_transform().location.z
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.vehicle_info_publisher.publish(vehicle_info)
def send_vehicle_msgs(self):
"""
Function (override) to send odometry message of the ego vehicle
instead of an object message.
The ego vehicle doesn't send its information as part of the object list.
A nav_msgs.msg.Odometry is prepared to be published
:return:
"""
vehicle_status = CarlaEgoVehicleStatus()
vehicle_status.header.stamp = self.get_current_ros_time()
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration = self.get_vehicle_acceleration_abs(
self.carla_actor)
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.publish_ros_message(self.topic_name() + "/vehicle_status",
vehicle_status)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.steer_angle = math.radians(wheel.steer_angle)
wheel_info.disable_steering = wheel.disable_steering
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.publish_ros_message(self.topic_name() + "/vehicle_info",
vehicle_info, True)
# @todo: do we still need this?
odometry = Odometry(header=self.get_msg_header())
odometry.child_frame_id = self.get_frame_id()
odometry.pose.pose = self.get_current_ros_pose()
odometry.twist.twist = self.get_current_ros_twist()
self.publish_ros_message(self.topic_name() + "/odometry", odometry)
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 __init__(self):
"""
Constructor
"""
super(CarlaAckermannControl, self).__init__("carla_ackermann_control")
# PID controller
# the controller has to run with the simulation time, not with real-time
#
# To prevent "float division by zero" within PID controller initialize it with
# a previous point in time (the error happens because the time doesn't
# change between initialization and first call, therefore dt is 0)
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: self.get_time() - 0.1)
# we might want to use a PID controller to reach the final target speed
self.speed_controller = PID(Kp=self.get_param("speed_Kp",
alternative_value=0.05),
Ki=self.get_param("speed_Ki",
alternative_value=0.),
Kd=self.get_param("speed_Kd",
alternative_value=0.5),
sample_time=0.05,
output_limits=(-1., 1.))
self.accel_controller = PID(Kp=self.get_param("accel_Kp",
alternative_value=0.05),
Ki=self.get_param("accel_Ki",
alternative_value=0.),
Kd=self.get_param("accel_Kd",
alternative_value=0.05),
sample_time=0.05,
output_limits=(-1, 1))
# use the correct time for further calculations
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: self.get_time())
if ROS_VERSION == 1:
self.reconfigure_server = Server(
EgoVehicleControlParameterConfig,
namespace="",
callback=self.reconfigure_pid_parameters,
)
if ROS_VERSION == 2:
self.add_on_set_parameters_callback(
self.reconfigure_pid_parameters)
self.control_loop_rate = self.get_param("control_loop_rate", 0.05)
self.last_ackermann_msg_received_sec = self.get_time()
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_info = CarlaEgoVehicleInfo()
self.role_name = self.get_param('role_name', 'ego_vehicle')
# control info
self.info = EgoVehicleControlInfo()
# set initial maximum values
self.vehicle_info_updated(self.vehicle_info)
# target values
self.info.target.steering_angle = 0.
self.info.target.speed = 0.
self.info.target.speed_abs = 0.
self.info.target.accel = 0.
self.info.target.jerk = 0.
# current values
self.info.current.time_sec = self.get_time()
self.info.current.speed = 0.
self.info.current.speed_abs = 0.
self.info.current.accel = 0.
# control values
self.info.status.status = 'n/a'
self.info.status.speed_control_activation_count = 0
self.info.status.speed_control_accel_delta = 0.
self.info.status.speed_control_accel_target = 0.
self.info.status.accel_control_pedal_delta = 0.
self.info.status.accel_control_pedal_target = 0.
self.info.status.brake_upper_border = 0.
self.info.status.throttle_lower_border = 0.
# control output
self.info.output.throttle = 0.
self.info.output.brake = 1.0
self.info.output.steer = 0.
self.info.output.reverse = False
self.info.output.hand_brake = True
# ackermann drive commands
self.control_subscriber = self.new_subscription(
AckermannDrive,
"/carla/" + self.role_name + "/ackermann_cmd",
self.ackermann_command_updated,
qos_profile=10)
# current status of the vehicle
self.vehicle_status_subscriber = self.new_subscription(
CarlaEgoVehicleStatus,
"/carla/" + self.role_name + "/vehicle_status",
self.vehicle_status_updated,
qos_profile=10)
# vehicle info
self.vehicle_info_subscriber = self.new_subscription(
CarlaEgoVehicleInfo,
"/carla/" + self.role_name + "/vehicle_info",
self.vehicle_info_updated,
qos_profile=10)
# to send command to carla
self.carla_control_publisher = self.new_publisher(
CarlaEgoVehicleControl,
"/carla/" + self.role_name + "/vehicle_control_cmd",
qos_profile=1)
# report controller info
self.control_info_publisher = self.new_publisher(
EgoVehicleControlInfo,
"/carla/" + self.role_name + "/ackermann_control/control_info",
qos_profile=1)
def __init__(self):
"""
Constructor
"""
self.control_loop_rate = rospy.Rate(10) # 10Hz
self.lastAckermannMsgReceived = datetime.datetime(
datetime.MINYEAR, 1, 1)
self.vehicle_status = CarlaEgoVehicleStatus()
self.vehicle_info = CarlaEgoVehicleInfo()
self.role_name = rospy.get_param('/carla/ackermann_control/role_name')
# control info
self.info = EgoVehicleControlInfo()
# set initial maximum values
self.vehicle_info_updated(self.vehicle_info)
# target values
self.info.target.steering_angle = 0.
self.info.target.speed = 0.
self.info.target.speed_abs = 0.
self.info.target.accel = 0.
self.info.target.jerk = 0.
# current values
self.info.current.time_sec = rospy.get_rostime().to_sec()
self.info.current.speed = 0.
self.info.current.speed_abs = 0.
self.info.current.accel = 0.
# control values
self.info.status.status = 'n/a'
self.info.status.speed_control_activation_count = 0
self.info.status.speed_control_accel_delta = 0.
self.info.status.speed_control_accel_target = 0.
self.info.status.accel_control_pedal_delta = 0.
self.info.status.accel_control_pedal_target = 0.
self.info.status.brake_upper_border = 0.
self.info.status.throttle_lower_border = 0.
# control output
self.info.output.throttle = 0.
self.info.output.brake = 1.0
self.info.output.steer = 0.
self.info.output.reverse = False
self.info.output.hand_brake = True
# PID controller
# the controller has to run with the simulation time, not with real-time
#
# To prevent "float division by zero" within PID controller initialize it with
# a previous point in time (the error happens because the time doesn't
# change between initialization and first call, therefore dt is 0)
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: rospy.get_rostime().to_sec() - 0.1)
# we might want to use a PID controller to reach the final target speed
self.speed_controller = PID(Kp=0.0,
Ki=0.0,
Kd=0.0,
sample_time=0.05,
output_limits=(-1., 1.))
self.accel_controller = PID(Kp=0.0,
Ki=0.0,
Kd=0.0,
sample_time=0.05,
output_limits=(-1, 1))
# use the correct time for further calculations
sys.modules['simple_pid.PID']._current_time = ( # pylint: disable=protected-access
lambda: rospy.get_rostime().to_sec())
self.reconfigure_server = Server(
EgoVehicleControlParameterConfig,
namespace="/carla/" + self.role_name + "/ackermann_control",
callback=(lambda config, level: CarlaAckermannControl.
reconfigure_pid_parameters(self, config, level)))
# ackermann drive commands
self.control_subscriber = rospy.Subscriber(
"/carla/" + self.role_name + "/ackermann_cmd", AckermannDrive,
self.ackermann_command_updated)
# current status of the vehicle
self.vehicle_status_subscriber = rospy.Subscriber(
"/carla/" + self.role_name + "/vehicle_status",
CarlaEgoVehicleStatus, self.vehicle_status_updated)
# vehicle info
self.vehicle_info_subscriber = rospy.Subscriber(
"/carla/" + self.role_name + "/vehicle_info", CarlaEgoVehicleInfo,
self.vehicle_info_updated)
# to send command to carla
self.carla_control_publisher = rospy.Publisher(
"/carla/" + self.role_name + "/vehicle_control_cmd",
CarlaEgoVehicleControl,
queue_size=1)
# report controller info
self.control_info_publisher = rospy.Publisher(
"/carla/" + self.role_name + "/ackermann_control/control_info",
EgoVehicleControlInfo,
queue_size=1)
def send_vehicle_msgs(self):
"""
send messages related to vehicle status
:return:
"""
vehicle_status = CarlaEgoVehicleStatus(
header=self.get_msg_header("map"))
vehicle_status.velocity = self.get_vehicle_speed_abs(self.carla_actor)
vehicle_status.acceleration.linear = transforms.carla_vector_to_ros_vector_rotated(
self.carla_actor.get_acceleration(),
self.carla_actor.get_transform().rotation)
vehicle_status.orientation = self.get_current_ros_pose().orientation
vehicle_status.control.throttle = self.carla_actor.get_control(
).throttle
vehicle_status.control.steer = self.carla_actor.get_control().steer
vehicle_status.control.brake = self.carla_actor.get_control().brake
vehicle_status.control.hand_brake = self.carla_actor.get_control(
).hand_brake
vehicle_status.control.reverse = self.carla_actor.get_control().reverse
vehicle_status.control.gear = self.carla_actor.get_control().gear
vehicle_status.control.manual_gear_shift = self.carla_actor.get_control(
).manual_gear_shift
self.publish_message(self.get_topic_prefix() + "/vehicle_status",
vehicle_status)
# only send vehicle once (in latched-mode)
if not self.vehicle_info_published:
self.vehicle_info_published = True
vehicle_info = CarlaEgoVehicleInfo()
vehicle_info.id = self.carla_actor.id
vehicle_info.type = self.carla_actor.type_id
vehicle_info.rolename = self.carla_actor.attributes.get(
'role_name')
vehicle_physics = self.carla_actor.get_physics_control()
for wheel in vehicle_physics.wheels:
wheel_info = CarlaEgoVehicleInfoWheel()
wheel_info.tire_friction = wheel.tire_friction
wheel_info.damping_rate = wheel.damping_rate
wheel_info.max_steer_angle = math.radians(
wheel.max_steer_angle)
vehicle_info.wheels.append(wheel_info)
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.max_rpm = vehicle_physics.max_rpm
vehicle_info.moi = vehicle_physics.moi
vehicle_info.damping_rate_full_throttle = vehicle_physics.damping_rate_full_throttle
vehicle_info.damping_rate_zero_throttle_clutch_engaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_engaged
vehicle_info.damping_rate_zero_throttle_clutch_disengaged = \
vehicle_physics.damping_rate_zero_throttle_clutch_disengaged
vehicle_info.use_gear_autobox = vehicle_physics.use_gear_autobox
vehicle_info.gear_switch_time = vehicle_physics.gear_switch_time
vehicle_info.clutch_strength = vehicle_physics.clutch_strength
vehicle_info.mass = vehicle_physics.mass
vehicle_info.drag_coefficient = vehicle_physics.drag_coefficient
vehicle_info.center_of_mass.x = vehicle_physics.center_of_mass.x
vehicle_info.center_of_mass.y = vehicle_physics.center_of_mass.y
vehicle_info.center_of_mass.z = vehicle_physics.center_of_mass.z
self.publish_message(self.get_topic_prefix() + "/vehicle_info",
vehicle_info, True)
# @todo: do we still need this?
odometry = Odometry(header=self.get_msg_header("map"))
odometry.child_frame_id = self.get_prefix()
odometry.pose.pose = self.get_current_ros_pose()
odometry.twist.twist = self.get_current_ros_twist()
self.publish_message(self.get_topic_prefix() + "/odometry", odometry)
