def provider_vehicle(position, heading, speed):
return VehicleState(
vehicle_id="sv-132",
vehicle_config_type="truck",
pose=Pose.from_center(position, heading),
dimensions=Dimensions(length=3, width=1, height=2),
speed=speed,
source="TESTS",
)
def _agent_spec_callback(self, ros_agent_spec: AgentSpec):
assert (len(ros_agent_spec.tasks) == 1
), "more than 1 task per agent is not yet supported"
task = ros_agent_spec.tasks[0]
task_params = json.loads(task.params_json) if task.params_json else {}
task_version = task.task_ver or "latest"
agent_locator = f"{self._zoo_module}:{task.task_ref}-{task_version}"
agent_spec = None
try:
agent_spec = registry.make(agent_locator, **task_params)
except ImportError as ie:
rospy.logerr(
f"Unable to locate agent with locator={agent_locator}: {ie}")
if not agent_spec:
rospy.logwarn(
f"got unknown task_ref '{task.task_ref}' in AgentSpec message with params='{task.param_json}'. ignoring."
)
return
if (ros_agent_spec.end_pose.position.x != 0.0
or ros_agent_spec.end_pose.position.y != 0.0):
goal = PositionalGoal(
(
ros_agent_spec.end_pose.position.x,
ros_agent_spec.end_pose.position.y,
),
ros_agent_spec.veh_length,
)
else:
goal = EndlessGoal()
mission = Mission(
start=Start.from_pose(
ROSDriver._pose_from_ros(ros_agent_spec.start_pose)),
goal=goal,
# TODO: how to prevent them from spawning on top of another existing vehicle? (see how it's done in SUMO traffic)
entry_tactic=default_entry_tactic(ros_agent_spec.start_speed),
vehicle_spec=VehicleSpec(
veh_id=f"veh_for_agent_{ros_agent_spec.agent_id}",
veh_config_type=ROSDriver._decode_vehicle_type(
ros_agent_spec.veh_type),
dimensions=Dimensions(
ros_agent_spec.veh_length,
ros_agent_spec.veh_width,
ros_agent_spec.veh_height,
),
),
)
with self._reset_lock:
if (ros_agent_spec.agent_id in self._agents
or ros_agent_spec.agent_id in self._agents_to_add):
rospy.logwarn(
f"trying to add new agent with existing agent_id '{ros_agent_spec.agent_id}'. ignoring."
)
return
self._agents_to_add[ros_agent_spec.agent_id] = (agent_spec,
mission)
def update_vehicle_state(self):
"""Update this vehicle state."""
assert len(self.vector) == 20
self.vs.pose = Pose.from_center(
self.vector[1:4], Heading(self.vector[4] % (2 * math.pi)))
self.vs.dimensions = Dimensions(*self.vector[5:8])
self.linear_velocity = self.vector[8:11]
self.angular_velocity = self.vector[11:14]
self.linear_acceleration = self.vector[14:17]
self.angular_acceleration = self.vector[17:]
self.vs.speed = np.linalg.norm(self.linear_velocity)
def test_vehicle_bounding_box(bullet_client):
pose = Pose.from_center((1, 1, 0), Heading(0))
chassis = BoxChassis(
pose=pose,
speed=0,
dimensions=Dimensions(length=3, width=1, height=1),
bullet_client=bullet_client,
)
vehicle = Vehicle(
id="vehicle-0",
chassis=chassis,
vehicle_config_type="passenger",
)
for coordinates in zip(vehicle.bounding_box, [[0.5, 2.5], (1.5, 2.5),
(1.5, -0.5), (0.5, -0.5)]):
assert np.array_equal(coordinates[0], coordinates[1])
def vehicle_dims(self, vehicle_id: str) -> Dimensions:
"""Get the vehicle dimensions of the specified vehicle."""
# do import here to break circular dependency chain
from smarts.core.vehicle import VEHICLE_CONFIGS
query = "SELECT length, width, height, type FROM Vehicle WHERE id = ?"
length, width, height, veh_type = self._query_val(
tuple, query, params=(vehicle_id, ))
default_dims = VEHICLE_CONFIGS[self.decode_vehicle_type(
veh_type)].dimensions
if not length:
length = default_dims.length
if not width:
width = default_dims.width
if not height:
height = default_dims.height
return Dimensions(length, width, height)
def test_create_social_vehicle(bullet_client):
chassis = BoxChassis(
pose=Pose.from_center((0, 0, 0), Heading(0)),
speed=0,
dimensions=Dimensions(length=3, width=1, height=1),
bullet_client=bullet_client,
)
car = Vehicle(
id="sv-132",
chassis=chassis,
vehicle_config_type="passenger",
)
assert car.vehicle_type == "car"
truck = Vehicle(
id="sv-132",
chassis=chassis,
vehicle_config_type="truck",
)
assert truck.vehicle_type == "truck"
def _entity_to_vs(entity: EntityState) -> VehicleState:
veh_id = entity.entity_id
veh_type = ROSDriver._decode_entity_type(entity.entity_type)
veh_dims = Dimensions(entity.length, entity.width, entity.height)
vs = VehicleState(
source="EXTERNAL",
vehicle_id=veh_id,
vehicle_config_type=veh_type,
pose=Pose(
ROSDriver._xyz_to_vect(entity.pose.position),
ROSDriver._xyzw_to_vect(entity.pose.orientation),
),
dimensions=veh_dims,
linear_velocity=ROSDriver._xyz_to_vect(entity.velocity.linear),
angular_velocity=ROSDriver._xyz_to_vect(entity.velocity.angular),
linear_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.linear),
angular_acceleration=ROSDriver._xyz_to_vect(
entity.acceleration.angular),
)
vs.set_privileged()
vs.speed = np.linalg.norm(vs.linear_velocity)
return vs
def _sync(self, provider_state: ProviderState):
provider_vehicles = {v.vehicle_id: v for v in provider_state.vehicles}
external_vehicles = [
v for v in provider_state.vehicles if v.source != "SUMO"
]
external_vehicle_ids = {v.vehicle_id for v in external_vehicles}
# Represents current state
traffic_vehicle_states = self._traci_conn.vehicle.getAllSubscriptionResults(
)
traffic_vehicle_ids = set(traffic_vehicle_states)
# State / ownership changes
external_vehicles_that_have_left = (self._non_sumo_vehicle_ids -
external_vehicle_ids -
traffic_vehicle_ids)
external_vehicles_that_have_joined = (external_vehicle_ids -
self._non_sumo_vehicle_ids -
traffic_vehicle_ids)
vehicles_that_have_become_external = (
traffic_vehicle_ids
& external_vehicle_ids - self._non_sumo_vehicle_ids)
# XXX: They may have become internal because they've been relinquished or
# because they've been destroyed from a collision. Presently we're not
# differentiating and will take over as social vehicles regardless.
vehicles_that_have_become_internal = (
self._non_sumo_vehicle_ids -
external_vehicle_ids) & traffic_vehicle_ids
log = ""
if external_vehicles_that_have_left:
log += (
f"external_vehicles_that_have_left={external_vehicles_that_have_left}\n"
)
if external_vehicles_that_have_joined:
log += f"external_vehicles_that_have_joined={external_vehicles_that_have_joined}\n"
if vehicles_that_have_become_external:
log += f"vehicles_that_have_become_external={vehicles_that_have_become_external}\n"
if vehicles_that_have_become_internal:
log += f"vehicles_that_have_become_internal={vehicles_that_have_become_internal}\n"
if log:
self._log.debug(log)
for vehicle_id in external_vehicles_that_have_left:
self._log.debug("Non SUMO vehicle %s left simulation", vehicle_id)
self._non_sumo_vehicle_ids.remove(vehicle_id)
self._traci_conn.vehicle.remove(vehicle_id)
for vehicle_id in external_vehicles_that_have_joined:
vehicle_state = provider_vehicles[vehicle_id]
dimensions = Dimensions.copy_with_defaults(
vehicle_state.dimensions,
VEHICLE_CONFIGS[vehicle_state.vehicle_config_type].dimensions,
)
self._create_vehicle(vehicle_id, dimensions)
no_checks = 0b00000
self._traci_conn.vehicle.setSpeedMode(vehicle_id, no_checks)
# update the state of all current managed vehicles
for vehicle_id in self._non_sumo_vehicle_ids:
provider_vehicle = provider_vehicles[vehicle_id]
pos, sumo_heading = provider_vehicle.pose.as_sumo(
provider_vehicle.dimensions.length, Heading(0))
# See https://sumo.dlr.de/docs/TraCI/Change_Vehicle_State.html#move_to_xy_0xb4
# for flag values
try:
self._move_vehicle(
provider_vehicle.vehicle_id,
pos,
sumo_heading,
provider_vehicle.speed,
)
except TraCIException as e:
# Likely as a result of https://github.com/eclipse/sumo/issues/3993
# the vehicle got removed because we skipped a moveToXY call between
# internal stepSimulations, so we add the vehicle back here.
self._log.warning(
"Attempted to (TraCI) SUMO.moveToXY(...) on missing "
f"vehicle(id={vehicle_id})")
self._create_vehicle(vehicle_id, provider_vehicle.dimensions)
self._move_vehicle(
provider_vehicle.vehicle_id,
pos,
sumo_heading,
provider_vehicle.speed,
)
for vehicle_id in vehicles_that_have_become_external:
no_checks = 0b00000
self._traci_conn.vehicle.setSpeedMode(vehicle_id, no_checks)
self._traci_conn.vehicle.setColor(
vehicle_id,
SumoTrafficSimulation._social_agent_vehicle_color())
self._non_sumo_vehicle_ids.add(vehicle_id)
for vehicle_id in vehicles_that_have_become_internal:
self._traci_conn.vehicle.setColor(
vehicle_id, SumoTrafficSimulation._social_vehicle_color())
self._non_sumo_vehicle_ids.remove(vehicle_id)
# Let sumo take over speed again
# For setSpeedMode look at: https://sumo.dlr.de/docs/TraCI/Change_Vehicle_State.html#speed_mode_0xb3
all_checks = 0b11111
self._traci_conn.vehicle.setSpeedMode(vehicle_id, all_checks)
self._traci_conn.vehicle.setSpeed(vehicle_id, -1)
if self._endless_traffic:
self._reroute_vehicles(traffic_vehicle_states)
self._teleport_exited_vehicles()
def __init__(
self,
pose: Pose,
bullet_client: bc.BulletClient,
vehicle_filepath=DEFAULT_VEHICLE_FILEPATH,
tire_parameters_filepath=None,
friction_map=None,
controller_parameters=DEFAULT_CONTROLLER_PARAMETERS,
initial_speed=None,
):
assert isinstance(pose, Pose)
self._log = logging.getLogger(self.__class__.__name__)
# XXX: Parameterize these vehicle properties?
self._client = bullet_client
self._chassis_aero_force_gain = controller_parameters["chassis"][
"chassis_aero_force_gain"]
self._max_brake_gain = controller_parameters["chassis"][
"max_brake_gain"]
# This value was found emperically. It causes the wheel steer joints to
# reach their maximum. We use this value to map to the -1, 1 steering range.
# If it were larger we'd cap out our turning radius before we hit -1, or 1.
# If it were smaller we'd never reach the tightest turning radius we could.
self._max_turn_radius = controller_parameters["chassis"][
"max_turn_radius"]
self._wheel_radius = controller_parameters["chassis"]["wheel_radius"]
self._max_torque = controller_parameters["chassis"]["max_torque"]
self._max_btorque = controller_parameters["chassis"]["max_btorque"]
# 720 is the maximum driver steering wheel angle
# which equals to two full rotation of steering wheel.
# This corresponds to maximum 41.3 deg angle at tires.
self._max_steering = controller_parameters["chassis"]["max_steering"]
self._steering_gear_ratio = controller_parameters["chassis"][
"steering_gear_ratio"]
self._tire_model = None
self._lat_forces = np.zeros(4)
self._lon_forces = np.zeros(4)
self._plane_id = None
self._friction_map = friction_map
self._tire_parameters = None
self._road_wheel_frictions = None
self._bullet_id = self._client.loadURDF(
vehicle_filepath,
[0, 0, 0],
[0, 0, 0, 1],
useFixedBase=False,
# We use cylinders for wheels, If we don't provide this flag, bullet
# will subdivide the cylinder resulting in bouncy ride
flags=pybullet.URDF_USE_IMPLICIT_CYLINDER
| pybullet.URDF_ENABLE_CACHED_GRAPHICS_SHAPES,
)
for i in range(self._client.getNumBodies()):
if self._client.getBodyInfo(i)[1].decode("ascii") == "plane":
self._plane_id = self._client.getBodyUniqueId(i)
self._road_wheel_frictions = {
"road_friction":
self._client.getDynamicsInfo(
self._plane_id,
-1,
)[1],
"wheel_friction":
self._client.getDynamicsInfo(
self._bullet_id,
2,
)[1],
}
break
self._controller_parameters = controller_parameters["control"]
if (tire_parameters_filepath
is not None) and os.path.exists(tire_parameters_filepath):
self._client.changeDynamics(
self._plane_id,
-1,
lateralFriction=1e-16,
)
with open(tire_parameters_filepath, "r") as tire_file:
self._tire_parameters = yaml.safe_load(tire_file)
self._tire_model = TireForces.build_tire_model(
[
self._tire_parameters["C_alpha_front"],
self._tire_parameters["C_alpha_rear"],
self._tire_parameters["C_x_front"],
self._tire_parameters["C_x_rear"],
],
self._tire_parameters["tire_model"],
self._tire_parameters["road_friction"],
)
for j in [2, 4, 5, 6]:
self._client.setCollisionFilterPair(self._bullet_id,
self._plane_id, j, -1, 0)
for _id in range(len(self._load_joints(self._bullet_id)) + 1):
self._client.changeDynamics(
self._bullet_id,
_id - 1,
linearDamping=0,
angularDamping=0.00001,
maxJointVelocity=
300, # This is a limit after which the pybullet interfer with the dynamics to ensure the bound.
)
width, length, height = np.array(
self._client.getCollisionShapeData(self._bullet_id, 0)[0][3])
self._dimensions = Dimensions(length=length,
width=width,
height=height)
chassis_pos = self._client.getLinkState(self._bullet_id, 0)[4]
center_offset = np.array(
self._client.getVisualShapeData(self._bullet_id, 0)[0][5])
self._joints = self._load_joints(self._bullet_id)
# 2,4,5,6 are the indices of wheels (FL,FR,RL,RR) and 1,3 are the
# indices for front left and front right steer joints, 0 is
# the index of the base link.
link_states = self._client.getLinkStates(self._bullet_id,
[0, 2, 4, 5, 6])
link_positions = [np.array(state[0]) for state in link_states]
base_pos = link_positions[0] + self._client.getLinkState(
self._bullet_id, 0)[2]
front_left_wheel_pos = link_positions[1]
front_right_wheel_pos = link_positions[2]
back_left_wheel_pos = link_positions[3]
back_right_wheel_pos = link_positions[4]
self._front_track_width = np.linalg.norm(front_left_wheel_pos -
front_right_wheel_pos)
self._rear_track_width = np.linalg.norm(back_left_wheel_pos -
back_right_wheel_pos)
self._front_distant_CG = np.linalg.norm(
0.5 * (front_left_wheel_pos[0:2] + front_right_wheel_pos[0:2]) -
base_pos[0:2])
self._rear_distant_CG = np.linalg.norm(
0.5 * (back_left_wheel_pos[0:2] + back_right_wheel_pos[0:2]) -
base_pos[0:2])
# distance between axles
front_axle_pos = (front_left_wheel_pos + front_right_wheel_pos) / 2
back_axle_pos = (back_left_wheel_pos + back_right_wheel_pos) / 2
self._wheel_base_length = np.linalg.norm(front_axle_pos -
back_axle_pos)
self._clear_joint_forces()
self.set_pose(pose)
if initial_speed is not None:
self._initialize_speed(initial_speed)