class Configuration(BaseModel):
# ROAR sensors settings
name: str = Field(default="hero",
title="Name of the agent",
description="Duplicate from Carla Setting. "
"But good to have")
front_depth_cam: Camera = Field(default=Camera(
fov=70,
transform=Transform(location=Location(x=1.6, y=0, z=1.7),
rotation=Rotation(pitch=0, yaw=0, roll=0)),
image_size_x=800,
image_size_y=600),
title="Front Depth Camera")
front_rgb_cam: Camera = Field(default=Camera(
fov=70,
transform=Transform(location=Location(x=1.6, y=0, z=1.7),
rotation=Rotation(pitch=0, yaw=0, roll=0)),
image_size_x=800,
image_size_y=600),
title="Front RGB Camera")
rear_rgb_cam: Camera = Field(default=Camera(
fov=145,
transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4),
rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)),
image_size_x=800,
image_size_y=600),
title="Rear RGB Camera")
# data path
waypoint_file_path: str = Field(
default=(Path(os.getcwd()) / "data" /
"easy_map_waypoints.txt").as_posix())
json_waypoint_file_path: str = Field(
default=(Path(os.getcwd()) / "data" /
"easy_map_waypoints.json").as_posix())
output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" /
"output"))
# miscellaneous settings
enable_autopilot: bool = Field(default=True,
title="Enable Antopilot",
description="Enable Antopilot")
spawn_point_id: int = Field(default=1,
title="Spaning Location ID",
description="Spanning Location ID")
show_sensors_data: bool = Field(default=False)
graph_post_modem_data: bool = Field(default=False)
save_sensor_data: bool = Field(default=False)
absolute_maximum_map_size: int = Field(
default=1000,
title="Absolute Maximum size of the map",
description="This variable is used to intialize the Occupancy grid map."
"The bigger it is, the more impact it is going to have on the runtime computation"
"However, if it is smaller than the actual map, some weird things can happen"
)
target_speed: int = 80
class Camera(BaseModel):
fov: int = Field(default=70, title="Field of View")
transform: Transform = Field(
default=Transform(
location=Location(x=1.6, y=0, z=1.7),
rotation=Rotation(pitch=0, yaw=0, roll=0),
)
)
image_size_x: int = Field(default=800, title="Image size width")
image_size_y: int = Field(default=600, title="Image size width")
distortion_coefficient: Optional[np.ndarray] = Field(default=np.array([0, 0, 0, 0, 0]),
title="Distortion Coefficient from Intel Realsense")
data: Optional[np.ndarray] = Field(default=None)
intrinsics_matrix: Optional[np.ndarray] = Field(default=None)
def calculate_default_intrinsics_matrix(self) -> np.ndarray:
"""
Calculate intrinsics matrix
Will set the attribute intrinsic matrix so that re-calculation is not
necessary.
https://github.com/carla-simulator/carla/issues/56
[
ax, 0, cx,
0, ay, cy,
0 , 0, 1
]
Returns:
Intrinsics_matrix
"""
intrinsics_matrix = np.identity(3)
intrinsics_matrix[0, 2] = self.image_size_x / 2.0
intrinsics_matrix[1, 2] = self.image_size_y / 2.0
intrinsics_matrix[0, 0] = self.image_size_x / (
2.0 * np.tan(self.fov * np.pi / 360.0)
)
intrinsics_matrix[1, 1] = self.image_size_y / (
2.0 * np.tan(self.fov * np.pi / 360.0)
)
self.intrinsics_matrix = intrinsics_matrix
return intrinsics_matrix
class Config:
arbitrary_types_allowed = True
def visualize(self, title="CameraData", duration=1) -> None:
"""
Visualize camera data.
Args:
title: title of cv2 image
duration: in milisecond
Returns:
None
"""
if self.data is not None:
cv2.imshow(title, self.data.data)
cv2.waitKey(duration)
def convert_rotation_from_source_to_agent(
self, source: carla.Rotation) -> Rotation:
"""Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float)."""
roll, pitch, yaw = source.roll, source.pitch, source.yaw
if yaw <= 90:
yaw = yaw + 90
else:
yaw = yaw - 270
return Rotation(roll=roll, pitch=pitch, yaw=-yaw)
def convert_vehicle_from_source_to_agent(self, source: JetsonVehicle) -> Vehicle:
return Vehicle(transform=Transform(
location=Location(x=-source.location[0], y=source.location[1], z=-source.location[2]),
rotation=Rotation(roll=-source.rotation[2], pitch=source.rotation[0], yaw=-source.rotation[1]),
),
velocity=Vector3D(x=-source.velocity[0], y=source.velocity[1], z=-source.velocity[2]),
wheel_base=0.26,
control=self.convert_control_from_source_to_agent(source=source)
)
def convert_rotation_from_source_to_agent(self,
source: np.ndarray) -> Rotation:
"""
Convert a Jetson raw rotation to Rotation(pitch=float,yaw=float,roll=float).
Args:
source ():
Returns:
Rotation(pitch, yaw, roll)
"""
return Rotation(roll=source[0], pitch=source[1], yaw=source[2])
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \
Optional[ViveTrackerData]:
if source is not None:
vive_tracker_data = ViveTrackerData(
location=Location(x=-source.x, y=source.y, z=-source.z),
rotation=Rotation(roll=-source.roll,
pitch=source.pitch + 60,
yaw=-source.yaw),
velocity=Vector3D(x=-source.vel_x,
y=source.vel_y,
z=-source.vel_z))
return vive_tracker_data
return None
def _raw_coord_to_transform(self, raw: List[float]) -> Optional[Transform]:
"""
transform coordinate to Transform instance
Args:
raw: coordinate in form of [x, y, z, pitch, yaw, roll]
Returns:
Transform instance
"""
if len(raw) == 3:
return Transform(
location=Location(x=raw[0], y=raw[1], z=raw[2]),
rotation=Rotation(pitch=0, yaw=0, roll=0),
)
elif len(raw) == 6:
return Transform(
location=Location(x=raw[0], y=raw[1], z=raw[2]),
rotation=Rotation(roll=raw[3], pitch=raw[4], yaw=raw[5]),
)
else:
self.logger.error(f"Point {raw} is invalid, skipping")
return None
def convert_rotation_from_source_to_agent(
self, source: carla.Rotation) -> Rotation:
"""Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float)."""
roll, pitch, yaw = source.roll, source.pitch, source.yaw
reflective_matrix = np.array([[0, 1, 0], [0, 0, -1], [1, 0, 0]])
R_Carla = rotation_matrix_from_euler(roll=roll, pitch=pitch, yaw=yaw)
# R_ROAR = P * R_carla * P
R_ROAR = reflective_matrix @ R_Carla @ reflective_matrix
r11, r21, r31, r32, r33 = R_ROAR[0][0], R_ROAR[1][0], R_ROAR[2][
0], R_ROAR[2][1], R_ROAR[2][2]
yaw = alpha = np.degrees(np.arctan2(r21, r11))
pitch = beta = np.degrees(np.arctan2(-r31, np.sqrt(r32**2 + r33**2)))
roll = gamma = np.degrees(np.arctan2(r32, r33))
return Rotation(roll=roll - 90, pitch=pitch, yaw=yaw)
def convert_vive_tracker_data_from_source_to_agent(self, source: Optional[ViveTrackerMessage]) -> \
Optional[ViveTrackerData]:
"""
Converts raw Vive Tracker data to ViveTrackerData(Location, Rotation, Velocity).
Args:
source ():
Returns:
ViveTrackerData(Location, Rotation, Velocity)
"""
if source is not None:
vive_tracker_data = ViveTrackerData(
location=Location(x=-source.x, y=source.y, z=-source.z),
rotation=Rotation(
roll=-source.roll,
pitch=source.pitch - 90, # 不知道为什么有60度的误差
yaw=-source.yaw),
velocity=Vector3D(x=source.vel_x,
y=source.vel_y,
z=source.vel_z))
return vive_tracker_data
return None
def rotationMatrixToEulerAngles(self, R) -> Rotation:
assert (self.isRotationMatrix(R))
sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
singular = sy < 1e-6
if not singular:
x = math.atan2(R[2, 1], R[2, 2])
y = math.atan2(-R[2, 0], sy)
z = math.atan2(R[1, 0], R[0, 0])
else:
x = math.atan2(-R[1, 2], R[1, 1])
y = math.atan2(-R[2, 0], sy)
z = 0
euler_angles_deg = np.rad2deg(np.array([x, y, z]))
rotation = Rotation(
roll=euler_angles_deg[2],
pitch=euler_angles_deg[0],
yaw=euler_angles_deg[1],
)
return rotation
def run_in_series(self) -> VehicleControl:
"""
Run step for the local planner
Procedure:
1. Sync data
2. get the correct look ahead for current speed
3. get the correct next waypoint
4. feed waypoint into controller
5. return result from controller
Returns:
next control that the local think the agent should execute.
"""
if (len(self.mission_planner.mission_plan) == 0
and len(self.way_points_queue) == 0):
return VehicleControl()
# get vehicle's location
vehicle_transform: Union[Transform,
None] = self.agent.vehicle.transform
if vehicle_transform is None:
raise AgentException(
"I do not know where I am, I cannot proceed forward")
# redefine closeness level based on speed
self.set_closeness_threhold(self.closeness_threshold_config)
# get current lane center
lane_detector: LaneDetector = self.agent.lane_detector
lane_center = lane_detector.lane_center
if lane_center is not None:
next_location = Location.from_array(lane_center[0] * 0.9 +
lane_center[1] * 0.1)
next_yaw, next_pitch = two_points_to_yaw_pitch(
lane_center[0], lane_center[1])
next_rotation = Rotation(yaw=next_yaw, pitch=next_pitch, roll=0)
target_waypoint = self.target_waypoint = Transform(
location=next_location, rotation=next_rotation)
else:
target_waypoint = self.target_waypoint
curr_closest_dist = float("inf")
while True:
if len(self.way_points_queue) == 0:
self.logger.info("Destination reached")
return VehicleControl()
# waypoint: Transform = self.way_points_queue[0]
waypoint: Transform = self.next_waypoint_smooth()
curr_dist = vehicle_transform.location.distance(waypoint.location)
if curr_dist < curr_closest_dist:
# if i find a waypoint that is closer to me than before
# note that i will always enter here to start the calculation for curr_closest_dist
curr_closest_dist = curr_dist
elif curr_dist < self.closeness_threshold:
# i have moved onto a waypoint, remove that waypoint from the queue
self.way_points_queue.popleft()
else:
break
target_waypoint = target_waypoint * 0.4 + self.next_waypoint_smooth(
) * 0.6
control: VehicleControl = self.controller.run_in_series(
next_waypoint=target_waypoint)
self.logger.debug(
f"\n"
f"Curr Transform: {self.agent.vehicle.transform}\n"
f"Target Location: {target_waypoint.location}\n"
f"Control: {control} | Speed: {Vehicle.get_speed(self.agent.vehicle)}\n"
)
return control
def convert_rotation_from_source_to_agent(self, source) -> Rotation:
return Rotation(pitch=source.pitch, yaw=source.yaw, roll=source.roll)
def convert_rotation_from_source_to_agent(self, source: carla.Rotation) -> Rotation:
"""Convert a CARLA raw rotation to Rotation(pitch=float,yaw=float,roll=float)."""
return Rotation(pitch=source.pitch, yaw=source.yaw, roll=source.roll)
class Configuration(BaseModel):
# ROAR sensors settings
name: str = Field(default="hero", title="Name of the agent", description="Duplicate from Carla Setting. "
"But good to have")
front_depth_cam: Camera = Field(default=Camera(fov=70,
transform=Transform(
location=Location(x=1.6,
y=0,
z=1.7
),
rotation=Rotation(
pitch=0,
yaw=0,
roll=0)),
image_size_x=800,
image_size_y=600),
title="Front Depth Camera")
front_rgb_cam: Camera = Field(default=Camera(fov=70,
transform=Transform(
location=Location(x=1.6,
y=0,
z=1.7),
rotation=Rotation(pitch=0,
yaw=0,
roll=0)
),
image_size_x=800,
image_size_y=600),
title="Front RGB Camera")
rear_rgb_cam: Camera = Field(default=Camera(fov=145,
transform=Transform(
location=Location(x=-1.5,
y=0.0,
z=1.4),
rotation=Rotation(
pitch=0.0, yaw=180,
roll=0.0)),
image_size_x=800,
image_size_y=600),
title="Rear RGB Camera")
# data path
waypoint_file_path: str = Field(default=(Path(
os.getcwd()) / "data" / "easy_map_waypoints.txt").as_posix())
json_waypoint_file_path: str = Field(default=(Path(
os.getcwd()) / "data" / "easy_map_waypoints.json").as_posix())
json_qr_code_file_path: str = Field(default=(Path(
os.getcwd()) / "ROAR_Jetson" / "data" / "track_1.json"
).as_posix())
output_data_folder_path: str = Field(
default=(Path(os.getcwd()) / "data" / "output"))
# miscellaneous settings
spawn_point_id: int = Field(default=1, title="Spaning Location ID",
description="Spanning Location ID")
show_sensors_data: bool = Field(default=False)
save_sensor_data: bool = Field(default=False)
absolute_maximum_map_size: int = Field(
default=1000, title="Absolute Maximum size of the map",
description="This variable is used to intialize the Occupancy grid map."
"The bigger it is, the more impact it is going to have on the runtime computation"
"However, if it is smaller than the actual map, some weird things can happen")
enable_autopilot: bool = Field(default=True, title="Enable Antopilot",
description="Enable Antopilot")
num_laps: int = Field(default=1, description="Number of laps to run for")
output_data_folder_path: str = Field(default="./data/output", description="path to save newly generated waypoints txt file.")
output_data_file_name: str = Field(default="map_waypoints", description="file name for a newly generated waypoints txt file.")
max_speed: float = Field(default=200, description="maximum speed in which the vehicle can drive at")
target_speed: int = Field(default=80, description="The tracking speed that the pid controller is trying to achieve")
steering_boundary: tuple = Field(default=(-1,1), description="maximum and minimum boundary for steering") # ROAR Academy:
throttle_boundary: tuple = Field(default=(0,1), description="maximum and minimum boundary for steering") # ROAR Academy:
waypoints_look_ahead_values: dict = Field(default={"60": 5, "80": 10, "120": 20, "180": 50}) # ROAR Academy:
simple_waypoint_local_planner_config_file_path: str = \
Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json")
pid_values: dict = Field(default={
'longitudinal_controller': {
'40': {
'Kp': 0.8, 'Kd': 0.2, 'Ki': 0
},
'60': {
'Kp': 0.5, 'Kd': 0.2, 'Ki': 0
},
'150': {
'Kp': 0.2, 'Kd': 0.1, 'Ki': 0.1
}
},
'latitudinal_controller': {
'60': {
'Kp': 0.8, 'Kd': 0.1, 'Ki': 0.1
},
'100': {
'Kp': 0.6, 'Kd': 0.2, 'Ki': 0.1
},
'150': {
'Kp': 0.5, 'Kd': 0.2, 'Ki': 0.1
}
}
}
) # ROAR Academy
pid_config_file_path: str = Field(default="./ROAR_Sim/configurations/pid_config.json")
lqr_config_file_path: str = Field(default="./ROAR_Sim/configurations/lqr_config.json")
def convert_rotation_from_source_to_agent(self,
source: Rotation) -> Rotation:
r = Rotation(roll=np.rad2deg(source.pitch),
pitch=np.rad2deg(source.roll),
yaw=np.rad2deg(source.yaw))
return r
class Configuration(BaseModel):
# ROAR sensors settings
name: str = Field(default="hero",
title="Name of the agent",
description="Duplicate from Carla Setting. "
"But good to have")
front_depth_cam: Camera = Field(default=Camera(
fov=70,
transform=Transform(location=Location(x=1.6, y=0, z=1.7),
rotation=Rotation(pitch=0, yaw=0, roll=0)),
image_size_x=800,
image_size_y=600),
title="Front Depth Camera")
front_rgb_cam: Camera = Field(default=Camera(
fov=70,
transform=Transform(location=Location(x=1.6, y=0, z=1.7),
rotation=Rotation(pitch=0, yaw=0, roll=0)),
image_size_x=800,
image_size_y=600),
title="Front RGB Camera")
rear_rgb_cam: Camera = Field(default=Camera(
fov=145,
transform=Transform(location=Location(x=-1.5, y=0.0, z=1.4),
rotation=Rotation(pitch=0.0, yaw=180, roll=0.0)),
image_size_x=800,
image_size_y=600),
title="Rear RGB Camera")
# data path
waypoint_file_path: str = Field(
default=(Path(os.getcwd()) / "data" /
"easy_map_waypoints.txt").as_posix())
json_waypoint_file_path: str = Field(
default=(Path(os.getcwd()) / "data" /
"easy_map_waypoints.json").as_posix())
json_qr_code_file_path: str = Field(default=(Path(os.getcwd()) /
"ROAR_Jetson" / "data" /
"track_1.json").as_posix())
output_data_folder_path: str = Field(default=(Path(os.getcwd()) / "data" /
"output"))
# miscellaneous settings
enable_autopilot: bool = Field(default=True,
title="Enable Antopilot",
description="Enable Antopilot")
spawn_point_id: int = Field(default=1,
title="Spaning Location ID",
description="Spanning Location ID")
show_sensors_data: bool = Field(default=False)
save_sensor_data: bool = Field(default=False)
absolute_maximum_map_size: int = Field(
default=1000,
title="Absolute Maximum size of the map",
description="This variable is used to intialize the Occupancy grid map."
"The bigger it is, the more impact it is going to have on the runtime computation"
"However, if it is smaller than the actual map, some weird things can happen"
)
target_speed: int = 80
pid_config_file_path: str = Field(
default="./ROAR_Sim/configurations/pid_config.json")
pid_config: dict = Field(default={60: {
1,
23,
}})
depth_to_pcd_config_path: str = Field(
default="./ROAR/configurations/carla/carla_depth_to_pcd_config.json")
lqr_config_file_path: str = Field(
default="./ROAR_Sim/configurations/lqr_config.json")
occu_map_config_path: str = Field(
default="./ROAR_Sim/configurations/occu_map_config.json")
obstacle_from_depth_config_path: str = Field(
default="./ROAR_Sim/configurations/obstacle_from_depth_config.json")
simple_waypoint_local_planner_config_file_path: str = \
Field(default="./ROAR_Sim/configurations/simple_waypoint_local_planner_config.json")
max_speed: float = Field(
default=200,
description="maximum speed in which the vehicle can drive at")
num_laps: int = Field(default=1, description="Number of laps to run for")