diff --git a/PythonAPI/carla/README.md b/PythonAPI/carla/README.md index 4cf6e36c6f..58ac59193a 100644 --- a/PythonAPI/carla/README.md +++ b/PythonAPI/carla/README.md @@ -2,4 +2,16 @@ 此包允许您控制 HUTB 模拟器并通过 Python API 检索模拟数据。例如，您可以控制模拟中的任何参与者（人、车、无人机、交通信号灯等），将传感器连接到参与者，并读取传感器数据等。 +## 开发 + +```shell +cd hutb/PythonAPI/carla +# 实现编辑模式安装 +# 修改 setup.py 后需要再次运行才会生效 +pip install -e . +# 测试 API +python ../util/config.py -l +python ../examples/holoocean/getting_started.py +``` + 更多信息，请参阅 [HUTB 文档](https://openhutb.github.io/) 。 diff --git a/PythonAPI/carla/setup.py b/PythonAPI/carla/setup.py index a4e5febc19..8450b3b626 100755 --- a/PythonAPI/carla/setup.py +++ b/PythonAPI/carla/setup.py @@ -196,10 +196,10 @@ def prune_rss(self): # also increase version number API_VERSION in Uitl/BuildTools/Windows.mk setup( name='hutb', - version='2.10.0', + version='2.10.1', author='OpenHUTB', package_dir={'': 'source'}, - packages=['carla', 'airsim', 'carla.ad', 'carla.ad.rss', 'carla.ad.map'] if is_rss_variant_enabled() else ['carla', 'airsim'], + packages=['carla', 'airsim', 'holoocean', 'carla.ad', 'carla.ad.rss', 'carla.ad.map'] if is_rss_variant_enabled() else ['carla', 'airsim', 'holoocean'], # For non-rss build do a fine grained include/exclude on the package data. package_data={'carla' : [""]}, exclude_package_data={} if is_rss_variant_enabled() else {'carla':[CleanADStubFiles.CARLA_RSS_STUB_FILE]}, @@ -216,3 +216,4 @@ def prune_rss(self): 'msgpack-rpc-python', 'numpy', 'opencv-contrib-python' ] ) +# pip uninstall pywin32 diff --git a/PythonAPI/carla/source/holoocean/__init__.py b/PythonAPI/carla/source/holoocean/__init__.py new file mode 100644 index 0000000000..8ea026ecce --- /dev/null +++ b/PythonAPI/carla/source/holoocean/__init__.py @@ -0,0 +1,8 @@ +"""HoloOcean is an underwater robotics simulator. +""" +__version__ = '1.0.0' + +from holoocean.holoocean import make +from holoocean.packagemanager import * + +__all__ = ['agents', 'environments', 'exceptions', 'holoocean', 'lcm', 'make', 'packagemanager', 'sensors'] diff --git a/PythonAPI/carla/source/holoocean/agents.py b/PythonAPI/carla/source/holoocean/agents.py new file mode 100644 index 0000000000..decd01959b --- /dev/null +++ b/PythonAPI/carla/source/holoocean/agents.py @@ -0,0 +1,925 @@ +"""Definitions for different agents that can be controlled from HoloOcean""" +from functools import reduce + +import numpy as np +from . import joint_constraints + +from holoocean.spaces import ContinuousActionSpace, DiscreteActionSpace +from holoocean.sensors import SensorDefinition, SensorFactory, RGBCamera +from holoocean.command import AddSensorCommand, RemoveSensorCommand + + +class ControlSchemes: + """All allowed control schemes. + + Attributes: + ANDROID_TORQUES (int): Default Android control scheme. Specify a torque for each joint. + CONTINUOUS_SPHERE_DEFAULT (int): Default ContinuousSphere control scheme. + Takes two commands, [forward_delta, turn_delta]. + DISCRETE_SPHERE_DEFAULT (int): Default DiscreteSphere control scheme. Takes a value, 0-4, + which corresponds with forward, backward, right, and left. + NAV_TARGET_LOCATION (int): Default NavAgent control scheme. Takes a target xyz coordinate. + UAV_TORQUES (int): Default UAV control scheme. Takes torques for roll, pitch, and yaw, as + well as thrust. + UAV_ROLL_PITCH_YAW_RATE_ALT (int): Control scheme for UAV. Takes roll, pitch, yaw rate, and + altitude targets. + HAND_AGENT_MAX_TORQUES (int): Default Android control scheme. Specify a torque for each joint. + AUV_THRUSTERS (int): Default HoveringAUV control scheme. Specify 8-vector of forces for each thruster. + AUV_CONTROL (int): Implemented PD controller. Specify 6-vector of position and roll,pitch,yaw to go too. + AUV_FORCES (int): Used for custom dynamics. All internal dynamics (except collisions) are turned off including + buoyancy, gravity, and damping. Specify 6-vector of linear and angular acceleration in the global frame. + TAUV_FINS (int): Default TorpedoAUV control scheme. Specify 5-vector of fin rotations in degrees and propeller value in Newtons. + TAUV_FORCES (int): Used for custom dynamics. All internal dynamics (except collisions) are turned off including + buoyancy, gravity, and damping. Specify 6-vector of linear and angular acceleration in the global frame. + SV_THRUSTERS (int): Default SurfaceVessel control scheme. Specify 2-vector of forces for left and right thruster. + SV_CONTROL (int): Implemented PD controller. Specify 2-vector of x and y position to go too. + SV_FORCES (int): Used for custom dynamics. All internal dynamics (except collisions) are turned off including + buoyancy, gravity, and damping. Specify 6-vector of linear and angular acceleration in the global frame. + """ + # Android Control Schemes + ANDROID_DIRECT_TORQUES = 0 + ANDROID_MAX_SCALED_TORQUES = 1 + + # Sphere Agent Control Schemes + SPHERE_DISCRETE = 0 + SPHERE_CONTINUOUS = 1 + + # Nav Agent Control Schemes + NAV_TARGET_LOCATION = 0 + + # Turtle agent + TURTLE_DIRECT_TORQUES = 0 + + # UAV Control Schemes + UAV_TORQUES = 0 + UAV_ROLL_PITCH_YAW_RATE_ALT = 1 + + # HandAgent Control Schemes + HAND_AGENT_MAX_TORQUES = 0 + HAND_AGENT_MAX_SCALED_TORQUES = 1 + HAND_AGENT_MAX_TORQUES_FLOAT = 2 + + # Hovering AUV Control Schemes + AUV_THRUSTERS = 0 + AUV_CONTROL = 1 + AUV_FORCES = 2 + + # Torpedo AUV Control Schemes + TAUV_FINS = 0 + TAUV_FORCES = 1 + + # Surface Vessel Control Schemes + SV_THRUSTERS = 0 + SV_CONTROL = 1 + SV_FORCES = 2 + +class HoloOceanAgent: + """A learning agent in HoloOcean + + Agents can act, receive rewards, and receive observations from their sensors. + Examples include the Android, UAV, and SphereRobot. + + Args: + client (:class:`~holoocean.holooceanclient.HoloOceanClient`): The HoloOceanClient that this + agent belongs with. + name (:obj:`str`, optional): The name of the agent. Must be unique from other agents in + the same environment. + sensors (:obj:`dict` of (:obj:`str`, :class:`~holoocean.sensors.HoloOceanSensor`)): A list + of HoloOceanSensors to read from this agent. + + Attributes: + name (:obj:`str`): The name of the agent. + sensors (dict of (string, :class:`~holoocean.sensors.HoloOceanSensor`)): List of + HoloOceanSensors on this agent. + agent_state_dict (dict): A dictionary that maps sensor names to sensor observation data. + """ + + def __init__(self, client, name="DefaultAgent"): + self.name = name + self._client = client + self.agent_state_dict = dict() + self.sensors = dict() + + self._num_control_schemes = len(self.control_schemes) + + self._max_control_scheme_length = \ + max(map(lambda x: reduce(lambda i, j: i * j, x[1].buffer_shape), + self.control_schemes)) + + self._action_buffer = \ + self._client.malloc(name, [self._max_control_scheme_length], np.float32) + # Teleport flag: 0: do nothing, 1: teleport, 2: rotate, 3: teleport and rotate + self._teleport_type_buffer = self._client.malloc(name + "_teleport_flag", [1], np.uint8) + self._teleport_buffer = self._client.malloc(name + "_teleport_command", [12], np.float32) + self._control_scheme_buffer = self._client.malloc(name + "_control_scheme", [1], + np.uint8) + self._current_control_scheme = 0 + self.set_control_scheme(0) + + def act(self, action): + """Sets the command for the agent. Action depends on the agent type and current control + scheme. + + Args: + action(:obj:`np.ndarray`): The action to take. + """ + self.__act__(action) + + def clear_action(self): + """Sets the action to zeros, effectively removing any previous actions. + """ + np.copyto(self._action_buffer, np.zeros(self._action_buffer.shape)) + + def set_control_scheme(self, index): + """Sets the control scheme for the agent. See :class:`ControlSchemes`. + + Args: + index (:obj:`int`): The control scheme to use. Should be set with an enum from + :class:`ControlSchemes`. + """ + self._current_control_scheme = index % self._num_control_schemes + self._control_scheme_buffer[0] = self._current_control_scheme + + def teleport(self, location=None, rotation=None): + """Teleports the agent to a specific location, with a specific rotation. + + Args: + location (np.ndarray, optional): An array with three elements specifying the target + world coordinates ``[x, y, z]`` in meters (see :ref:`coordinate-system`). + + If ``None`` (default), keeps the current location. + rotation (np.ndarray, optional): An array with three elements specifying roll, pitch, + and yaw in degrees of the agent. + + If ``None`` (default), keeps the current rotation. + + """ + val = 0 + if location is not None: + val += 1 + np.copyto(self._teleport_buffer[0:3], location) + if rotation is not None: + np.copyto(self._teleport_buffer[3:6], rotation) + val += 2 + self._teleport_type_buffer[0] = val + + def set_physics_state(self, location, rotation, velocity, angular_velocity): + """Sets the location, rotation, velocity and angular velocity of an agent. + + Args: + location (np.ndarray): New location (``[x, y, z]`` (see :ref:`coordinate-system`)) + rotation (np.ndarray): New rotation (``[roll, pitch, yaw]``, see (see :ref:`rotations`)) + velocity (np.ndarray): New velocity (``[x, y, z]`` (see :ref:`coordinate-system`)) + angular_velocity (np.ndarray): New angular velocity (``[x, y, z]`` in **degrees** + (see :ref:`coordinate-system`)) + + """ + np.copyto(self._teleport_buffer[0:3], location) + np.copyto(self._teleport_buffer[3:6], rotation) + np.copyto(self._teleport_buffer[6:9], velocity) + np.copyto(self._teleport_buffer[9:12], angular_velocity) + self._teleport_type_buffer[0] = 15 + + def add_sensors(self, sensor_defs): + """Adds a sensor to a particular agent object and attaches an instance of the sensor to the + agent in the world. + + Args: + sensor_defs (:class:`~holoocean.sensors.HoloOceanSensor` or + list of :class:`~holoocean.sensors.HoloOceanSensor`): + Sensors to add to the agent. + """ + if not isinstance(sensor_defs, list): + sensor_defs = [sensor_defs] + + for sensor_def in sensor_defs: + if sensor_def.agent_name == self.name: + sensor = SensorFactory.build_sensor(self._client, sensor_def) + self.sensors[sensor_def.sensor_name] = sensor + self.agent_state_dict[sensor_def.sensor_name] = sensor.sensor_data + + if not sensor_def.existing: + command_to_send = AddSensorCommand(sensor_def) + self._client.command_center.enqueue_command(command_to_send) + + def remove_sensors(self, sensor_defs): + """Removes a sensor from a particular agent object and detaches it from the agent in the + world. + + Args: + sensor_defs (:class:`~holoocean.sensors.HoloOceanSensor` or + list of :class:`~holoocean.sensors.HoloOceanSensor`): + Sensors to remove from the agent. + """ + if not isinstance(sensor_defs, list): + sensor_defs = [sensor_defs] + + for sensor_def in sensor_defs: + self.sensors.pop(sensor_def.sensor_name, None) + self.agent_state_dict.pop(sensor_def.sensor_name, None) + command_to_send = RemoveSensorCommand(self.name, sensor_def.sensor_name) + self._client.command_center.enqueue_command(command_to_send) + + def has_camera(self): + """Indicatates whether this agent has a camera or not. + + Returns: + :obj:`bool`: If the agent has a sensor or not + """ + for sensor_type in self.sensors.items(): + if sensor_type is RGBCamera: + return True + + return False + + @property + def action_space(self): + """Gets the action space for the current agent and control scheme. + + Returns: + :class:`~holoocean.spaces.ActionSpace`: The action space for this agent and control + scheme.""" + return self.control_schemes[self._current_control_scheme][1] + + @property + def control_schemes(self): + """A list of all control schemes for the agent. Each list element is a 2-tuple, with the + first element containing a short description of the control scheme, and the second + element containing the :obj:`ActionSpace` for the control scheme. + + Returns: + (:obj:`str`, :class:`~holoocean.spaces.ActionSpace`): + Each tuple contains a short description and the ActionSpace + """ + raise NotImplementedError("Child class must implement this function") + + def get_joint_constraints(self, joint_name): + """Returns the corresponding swing1, swing2 and twist limit values for the + specified joint. Will return None if the joint does not exist for the agent. + """ + raise NotImplementedError("Child class must implement this function") + + def __act__(self, action): + # Allow for smaller arrays to be provided as input + if len(self._action_buffer) > len(action): + action = np.copy(action) + action.resize(self._action_buffer.shape) + + # The default act function is to copy the data, + # but if needed it can be overridden + np.copyto(self._action_buffer, action) + + def __repr__(self): + return self.name + + +class UavAgent(HoloOceanAgent): + """A UAV (quadcopter) agent + + **Action Space:** + + Has two possible continuous action control schemes + + 1. [pitch_torque, roll_torque, yaw_torque, thrust] and + 2. [pitch_target, roll_target, yaw_rate_target, altitude_target] + + See :ref:`uav-agent` for more details. + + Inherits from :class:`HoloOceanAgent`. + """ + # constants in Uav.h in holoocean-engine + __MAX_ROLL = 6.5080 + __MIN_ROLL = -__MAX_ROLL + + __MAX_PITCH = 5.087 + __MIN_PITCH = -__MAX_PITCH + + __MAX_YAW_RATE = .8 + __MIN_YAW_RATE = -__MAX_YAW_RATE + + __MAX_FORCE = 59.844 + __MIN_FORCE = -__MAX_FORCE + + agent_type = "UAV" + + @property + def control_schemes(self): + torques_min = [self.__MIN_PITCH, self.__MIN_ROLL, self.__MIN_YAW_RATE, self.__MIN_FORCE] + torques_max = [self.__MAX_PITCH, self.__MAX_ROLL, self.__MAX_YAW_RATE, self.__MAX_FORCE] + no_min_max = [None, None, None, None] + return [("[pitch_torque, roll_torque, yaw_torque, thrust]", + ContinuousActionSpace([4], low=torques_min, high=torques_max)), + ("[pitch_target, roll_target, yaw_rate_target, altitude_target]", + ContinuousActionSpace([4], low=no_min_max, high=no_min_max))] + + def __repr__(self): + return "UavAgent " + self.name + + def get_joint_constraints(self, joint_name): + return None + + +class SphereAgent(HoloOceanAgent): + """A basic sphere robot. + + See :ref:`sphere-agent` for more details. + + **Action Space:** + + Has two possible control schemes, one discrete and one continuous: + + +-------------------+---------+----------------------+ + | Control Scheme | Value | Action | + +-------------------+---------+----------------------+ + | Discrete (``0``) | ``[0]`` | Move forward | + | +---------+----------------------+ + | | ``[1]`` | Move backward | + | +---------+----------------------+ + | | ``[2]`` | Turn right | + | +---------+----------------------+ + | | ``[3]`` | Turn left | + +-------------------+---------+----------------------+ + | Continuous (``1``)| ``[forward_speed, rot_speed]`` | + +-------------------+--------------------------------+ + + Inherits from :class:`HoloOceanAgent`. + """ + # constants in SphereRobot.h in holoocean-engine + __DISCRETE_MIN = 0 + __DISCRETE_MAX = 4 + + __MAX_ROTATION_SPEED = 20 + __MIN_ROTATION_SPEED = -__MAX_ROTATION_SPEED + + __MAX_FORWARD_SPEED = 20 + __MIN_FORWARD_SPEED = -__MAX_FORWARD_SPEED + + agent_type = "SphereRobot" + + @property + def control_schemes(self): + cont_min = [self.__MIN_FORWARD_SPEED, self.__MIN_ROTATION_SPEED] + cont_max = [self.__MAX_FORWARD_SPEED, self.__MAX_ROTATION_SPEED] + return [("0: Move forward\n1: Move backward\n2: Turn right\n3: Turn left", + DiscreteActionSpace([1], low=self.__DISCRETE_MIN, high=self.__DISCRETE_MAX, buffer_shape=[2])), + ("[forward_movement, rotation]", ContinuousActionSpace([2], low=cont_min, high=cont_max))] + + def get_joint_constraints(self, joint_name): + return None + + def __act__(self, action): + if self._current_control_scheme is ControlSchemes.SPHERE_CONTINUOUS: + np.copyto(self._action_buffer, action) + elif self._current_control_scheme is ControlSchemes.SPHERE_DISCRETE: + # Move forward .185 meters (to match initial release) + # Turn 10/-10 degrees (to match initial release) + actions = np.array([[.185, 0], [-.185, 0], [0, 10], [0, -10]]) + to_act = np.array(actions[action, :]) + np.copyto(self._action_buffer, to_act) + + def __repr__(self): + return "SphereAgent " + self.name + + +class AndroidAgent(HoloOceanAgent): + """An humanoid android agent. + + Can be controlled via torques supplied to its joints. + + **Action Space:** + + 94 dimensional vector of continuous values representing torques to be + applied at each joint. The layout of joints can be found here: + + There are 18 joints with 3 DOF, 10 with 2 DOF, and 20 with 1 DOF. + + Inherits from :class:`HoloOceanAgent`.""" + # constants in Android.h in holoocean-engine + __MAX_TORQUE = 20 + __MIN_TORQUE = -__MAX_TORQUE + __JOINTS_VECTOR_SIZE = 94 + + agent_type = "Android" + + @property + def control_schemes(self): + direct_min = [self.__MIN_TORQUE for _ in range(self.__JOINTS_VECTOR_SIZE)] + direct_max = [self.__MAX_TORQUE for _ in range(self.__JOINTS_VECTOR_SIZE)] + scaled_min = [-1 for _ in range(self.__JOINTS_VECTOR_SIZE)] + scaled_max = [1 for _ in range(self.__JOINTS_VECTOR_SIZE)] + + return [("[Raw Bone Torques] * 94", + ContinuousActionSpace([self.__JOINTS_VECTOR_SIZE], low=direct_min, high=direct_max)), + ("[-1 to 1] * 94, where 1 is the maximum torque for a given " + "joint (based on mass of bone)", + ContinuousActionSpace([self.__JOINTS_VECTOR_SIZE], low=scaled_min, high=scaled_max))] + + def __repr__(self): + return "AndroidAgent " + self.name + + @staticmethod + def joint_ind(joint_name): + """Gets the joint indices for a given name + + Args: + joint_name (:obj:`str`): Name of the joint to look up + + Returns: + (int): The index into the state array + """ + return AndroidAgent._joint_indices[joint_name] + + def get_joint_constraints(self, joint_name): + if joint_name in joint_constraints.android_agent_joints_constraints: + return joint_constraints.android_agent_joints_constraints[joint_name] + return None + + _joint_indices = { + # Head, Spine, and Arm joints. Each has[swing1, swing2, twist] + "head": 0, + "neck_01": 3, + "spine_02": 6, + "spine_01": 9, + "upperarm_l": 12, + "lowerarm_l": 15, + "hand_l": 18, + "upperarm_r": 21, + "lowerarm_r": 24, + "hand_r": 27, + + # Leg Joints. Each has[swing1, swing2, twist] + "thigh_l": 30, + "calf_l": 33, + "foot_l": 36, + "ball_l": 39, + "thigh_r": 42, + "calf_r": 45, + "foot_r": 48, + "ball_r": 51, + + # First joint of each finger. Has only [swing1, swing2] + "thumb_01_l": 54, + "index_01_l": 56, + "middle_01_l": 58, + "ring_01_l": 60, + "pinky_01_l": 62, + "thumb_01_r": 64, + "index_01_r": 66, + "middle_01_r": 68, + "ring_01_r": 70, + "pinky_01_r": 72, + + # Second joint of each finger. Has only[swing1] + "thumb_02_l": 74, + "index_02_l": 75, + "middle_02_l": 76, + "ring_02_l": 77, + "pinky_02_l": 78, + "thumb_02_r": 79, + "index_02_r": 80, + "middle_02_r": 81, + "ring_02_r": 82, + "pinky_02_r": 83, + + # Third joint of each finger. Has only[swing1] + "thumb_03_l": 84, + "index_03_l": 85, + "middle_03_l": 86, + "ring_03_l": 87, + "pinky_03_l": 88, + "thumb_03_r": 89, + "index_03_r": 90, + "middle_03_r": 91, + "ring_03_r": 92, + "pinky_03_r": 93 + } + + +class HandAgent(HoloOceanAgent): + """A floating hand agent. + + Can be controlled via torques supplied to its joints and moved around in + three dimensions. + + **Action Space:** + + 23 or 26 dimensional vector of continuous values representing torques to be + applied at each joint. + + Inherits from :class:`HoloOceanAgent`. + + """ + # constants in HandAgent.h in holoocean-engine + __MAX_MOVEMENT_METERS = 0.5 + __MIN_MOVEMENT_METERS = -__MAX_MOVEMENT_METERS + + __MAX_TORQUE = 20 + __MIN_TORQUE = -__MAX_TORQUE + + __JOINTS_DOF = 23 + __JOINTS_AND_DIST = 26 + + agent_type = "HandAgent" + + @property + def control_schemes(self): + raw_min = [self.__MIN_TORQUE for _ in range(self.__JOINTS_DOF)] + raw_max = [self.__MAX_TORQUE for _ in range(self.__JOINTS_DOF)] + joint_min = [-1 for _ in range(self.__JOINTS_DOF)] + joint_max = [1 for _ in range(self.__JOINTS_DOF)] + + scaled_min = [-1.0 if i < self.__JOINTS_DOF + else self.__MIN_MOVEMENT_METERS for i in range(self.__JOINTS_AND_DIST)] + scaled_max = [1.0 if i < self.__JOINTS_DOF + else self.__MAX_MOVEMENT_METERS for i in range(self.__JOINTS_AND_DIST)] + + return [("[Raw Bone Torques] * 23", ContinuousActionSpace([self.__JOINTS_DOF], low=raw_min, high=raw_max)), + ("[-1 to 1] * 23, where 1 is the maximum torque for the given index" + "joint (based on mass of bone)", + ContinuousActionSpace([self.__JOINTS_DOF], low=joint_min, high=joint_max)), + ("[-1 to 1] * 23, scaled torques, then [x, y, z] transform", + ContinuousActionSpace([self.__JOINTS_AND_DIST], low=scaled_min, high=scaled_max))] + + def __repr__(self): + return "HandAgent " + self.name + + @staticmethod + def joint_ind(joint_name): + """Gets the joint indices for a given name + + Args: + joint_name (:obj:`str`): Name of the joint to look up + + Returns: + (int): The index into the state array + """ + return HandAgent._joint_indices[joint_name] + + def get_joint_constraints(self, joint_name): + if joint_name in joint_constraints.hand_agent_joints_constraints: + return joint_constraints.hand_agent_joints_constraints[joint_name] + return None + + _joint_indices = { + # Head, Spine, and Arm joints. Each has[swing1, swing2, twist] + "hand_r": 0, + + # First joint of each finger. Has only [swing1, swing2] + "thumb_01_r": 3, + "index_01_r": 5, + "middle_01_r": 7, + "ring_01_r": 9, + "pinky_01_r": 11, + + # Second joint of each finger. Has only[swing1] + "thumb_02_r": 12, + "index_02_r": 13, + "middle_02_r": 14, + "ring_02_r": 15, + "pinky_02_r": 16, + + # Third joint of each finger. Has only[swing1] + "thumb_03_r": 17, + "index_03_r": 18, + "middle_03_r": 19, + "ring_03_r": 20, + "pinky_03_r": 21 + } + + +class NavAgent(HoloOceanAgent): + """A humanoid character capable of intelligent navigation. + + **Action Space:** + + Continuous control scheme of the form ``[x_target, y_target, z_target]``. + (see :ref:`coordinate-system`) + + Inherits from :class:`HoloOceanAgent`. + + """ + + # constants in NavAgent.h in holoocean-engine + __MAX_DISTANCE = 0.5 + __MIN_DISTANCE = -__MAX_DISTANCE + + agent_type = "NavAgent" + + @property + def control_schemes(self): + low = [self.__MIN_DISTANCE for _ in range(3)] + high = [self.__MAX_DISTANCE for _ in range(3)] + return [("[x_target, y_target, z_target]", ContinuousActionSpace([3], low=low, high=high))] + + def get_joint_constraints(self, joint_name): + return None + + def __repr__(self): + return "NavAgent " + self.name + + def __act__(self, action): + np.copyto(self._action_buffer, np.array(action)) + + +class TurtleAgent(HoloOceanAgent): + """A simple turtle bot. + + **Action Space**: + + ``[forward_force, rot_force]`` + + - ``forward_force`` is capped at 160 in either direction + - ``rot_force`` is capped at 35 either direction + + Inherits from :class:`HoloOceanAgent`.""" + # constants in TurtleAgent.h in holoocean-engine + __MAX_THRUST = 160.0 + __MIN_THRUST = -__MAX_THRUST + + __MAX_YAW = 35.0 + __MIN_YAW = -__MAX_YAW + + agent_type = "TurtleAgent" + + @property + def control_schemes(self): + low = [self.__MIN_THRUST, self.__MIN_YAW] + high = [self.__MAX_THRUST, self.__MAX_YAW] + return [("[forward_force, rot_force]", ContinuousActionSpace([2], low=low, high=high))] + + def get_joint_constraints(self, joint_name): + return None + + def __repr__(self): + return "TurtleAgent " + self.name + + def __act__(self, action): + np.copyto(self._action_buffer, np.array(action)) + np.copyto(self._action_buffer, action) + + +class HoveringAUV(HoloOceanAgent): + """A simple autonomous underwater vehicle. All variables are not actually used in simulation, + modifying them will have no effect on results. They are exposed for convenience in implementing custom + dynamics. + + **Action Space** + + Has three possible control schemes, as follows + + + #. Thruster Forces: ``[Vertical Front Starboard, Vertical Front Port, Vertical Back Port, Vertical Back Starboard, Angled Front Starboard, Angled Front Port, Angled Back Port, Angled Back Starboard]`` + + #. PD Controller: ``[des_pos_x, des_pos_y, des_pos_z, roll, pitch, yaw]`` + + #. Accelerations, in global frame: ``[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]`` + + Inherits from :class:`HoloOceanAgent`. + + + :cvar mass: (:obj:`float`): Mass of the vehicle in kg. + :cvar water_density: (:obj:`float`): Water density in kg / m^3. + :cvar volume: (:obj:`float`): Volume of vehicle in m^3. + :cvar cob: (:obj:`np.ndarray`): 3-vecter Center of buoyancy from the center of mass in m. + :cvar I: (:obj:`np.ndarray`): 3x3 Inertia matrix. + :cvar thruster_d: (:obj:`np.ndarray`): 8x3 matrix of unit vectors in the direction of thruster propulsion + :cvar thruster_p: (:obj:`np.ndarray`): 8x3 matrix of positions in local frame of thrusters positions in m.""" + # constants in HoveringAUV.h in holoocean-engine + __MAX_LIN_ACCEL = 10 + __MAX_ANG_ACCEL = 2 + __MAX_THRUST = __MAX_LIN_ACCEL*31.02/4 + + agent_type = "HoveringAUV" + + mass = 31.02 + water_density = 997 + volume = mass / water_density + cob = np.array([0,0,.05]) + I = np.eye(3) + + thruster_d = np.array([[0, 0, 1], + [0, 0, 1], + [0, 0, 1], + [0, 0, 1], + [1/np.sqrt(2), 1/np.sqrt(2), 0], + [1/np.sqrt(2), -1/np.sqrt(2), 0], + [1/np.sqrt(2), 1/np.sqrt(2), 0], + [1/np.sqrt(2), -1/np.sqrt(2), 0]]) + + thruster_p = np.array([[0.25, -0.22, -0.04], + [0.25, 0.22, -0.04], + [-0.25, 0.22, -0.04], + [-0.25, -0.22, -0.04], + [0.14, -0.18, 0], + [0.14, 0.18, 0], + [-0.14, 0.18, 0], + [-0.14, -0.18, 0]]) + + @property + def control_schemes(self): + scheme_thrusters = "[Vertical Front Starboard, Vertical Front Port, Vertical Back Port, Vertical Back Starboard, Angled Front Starboard, Angled Front Port, Angled Back Port, Angled Back Starboard]" + + scheme_accel = "[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]" + limits_accel = [self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL] + + scheme_control = "[des_x, des_y, des_z, des_roll, des_pitch, des_yaw]" + limits_control = [np.NaN, np.NaN, np.NaN, 180, 90, 180] + + return [(scheme_thrusters, ContinuousActionSpace([8], low=[-self.__MAX_THRUST]*8, high=[self.__MAX_THRUST]*8)), + (scheme_accel, ContinuousActionSpace([6], low=[-i for i in limits_accel], high=limits_accel)), + (scheme_control, ContinuousActionSpace([6], low=[-i for i in limits_control], high=limits_control))] + + def get_joint_constraints(self, joint_name): + return None + + def __repr__(self): + return "HoveringAUV " + self.name + + +class SurfaceVessel(HoloOceanAgent): + """A simple surface vessel. All variables are not actually used in simulation, + modifying them will have no effect on results. They are exposed for convenience in implementing custom + dynamics. + + **Action Space** + + Has three possible control schemes, as follows + + + #. Thruster Forces: ``[Left thruster, Right thruster]`` + + #. PD Controller: ``[des_x, des_y, des_yaw]`` + + #. Accelerations, in global frame: ``[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]`` + + Inherits from :class:`HoloOceanAgent`. + + + :cvar mass: (:obj:`float`): Mass of the vehicle in kg. + :cvar water_density: (:obj:`float`): Water density in kg / m^3. + :cvar volume: (:obj:`float`): Volume of vehicle in m^3. + :cvar cob: (:obj:`np.ndarray`): 3-vecter Center of buoyancy from the center of mass in m. + :cvar I: (:obj:`np.ndarray`): 3x3 Inertia matrix. + :cvar thruster_p: (:obj:`np.ndarray`): 2x3 matrix of positions in local frame of thrusters positions in m.""" + # constants in SurfaceVessel.h in holoocean-engine + __MAX_LIN_ACCEL = 20 + __MAX_ANG_ACCEL = 2 + __MAX_THRUST = 1500 + + agent_type = "SurfaceVessel" + + mass = 200 + water_density = 997 + volume = 6 * mass / water_density + cob = np.array([0,0,.1]) + I = np.diag([2,2,1]) + + thruster_p = np.array([[-250, -100, -0], [-250, 100, -0]]) / 100 + + @property + def control_schemes(self): + scheme_thrusters = "[Left thruster, Right thruster]" + + scheme_accel = "[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]" + limits_accel = [self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL] + + scheme_control = "[des_x, des_y]" + limits_control = [np.NaN, np.NaN] + + return [(scheme_thrusters, ContinuousActionSpace([2], low=[-self.__MAX_THRUST]*8, high=[self.__MAX_THRUST]*8)), + (scheme_accel, ContinuousActionSpace([6], low=[-i for i in limits_accel], high=limits_accel)), + (scheme_control, ContinuousActionSpace([2], low=[-i for i in limits_control], high=limits_control))] + + def get_joint_constraints(self, joint_name): + return None + + def __repr__(self): + return "SurfaceVessel " + self.name + + +class TorpedoAUV(HoloOceanAgent): + """A simple foward motion autonomous underwater vehicle. All variables are not actually used in simulation, + modifying them will have no effect on results. They are exposed for convenience in implementing custom + dynamics. + + **Action Space** + + Has two possible action spaces, as follows: + + #. Fins & Propeller: ``[left_fin, top_fin, right_fin, bottom_fin, thrust]`` + + #. Accelerations, in global frame: ``[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]`` + + Inherits from :class:`HoloOceanAgent`. + + :cvar mass: (:obj:`float`): Mass of the vehicle in kg. + :cvar water_density: (:obj:`float`): Water density in kg / m^3. + :cvar volume: (:obj:`float`): Volume of vehicle in m^3. + :cvar cob: (:obj:`np.ndarray`): 3-vecter Center of buoyancy from the center of mass in m. + :cvar I: (:obj:`np.ndarray`): 3x3 Inertia matrix. + :cvar thruster_p: (:obj:`np.ndarray`): 3 matrix of positions in local frame of propeller position in m. + :cvar fin_p: (:obj:`np.ndarray`): 4x3 matrix of positions in local frame of fin positions in m.""" + # constants in TorpedoAUV.h in holoocean-engine + __MAX_THRUST = 100 + __MAX_FIN = 45 + __MAX_LIN_ACCEL = 10 + __MAX_ANG_ACCEL = 2 + + agent_type = "TorpedoAUV" + + mass = 36 + water_density = 997 + volume = mass / water_density + cob = np.array([0,0,.07]) + I = np.diag([2, 1.2, 1.2]) + + thruster_p = np.array([-120, 0, 0]) / 100 + + fin_p = np.array( [[-105,-7.07, 0], + [-105, 0, 7.07], + [-105, 7.07, 0], + [-105, 0,-7.07]]) / 100 + + @property + def control_schemes(self): + scheme_fins = "[right_fin, top_fin, left_fin, bottom_fin, thrust]" + limits_fins = [self.__MAX_FIN]*4 + [self.__MAX_THRUST] + + scheme_accel = "[lin_accel_x, lin_accel_y, lin_accel_z, ang_accel_x, ang_accel_y, ang_accel_x]" + limits_accel = [self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_LIN_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL, self.__MAX_ANG_ACCEL] + + return [(scheme_fins, ContinuousActionSpace([5], low=[-i for i in limits_fins], high=limits_fins)), + (scheme_accel, ContinuousActionSpace([6], low=[-i for i in limits_accel], high=limits_accel))] + + def get_joint_constraints(self, joint_name): + return None + + def __repr__(self): + return "TorpedoAUV " + self.name + + +class AgentDefinition: + """Represents information needed to initialize agent. + + Args: + agent_name (:obj:`str`): The name of the agent to control. + agent_type (:obj:`str` or type): The type of HoloOceanAgent to control, string or class + reference. + sensors (:class:`~holoocean.sensors.SensorDefinition` or class type (if no duplicate sensors)): A list of + HoloOceanSensors to read from this agent. + starting_loc (:obj:`list` of :obj:`float`): Starting ``[x, y, z]`` location for agent + (see :ref:`coordinate-system`) + starting_rot (:obj:`list` of :obj:`float`): Starting ``[roll, pitch, yaw]`` rotation for agent + (see :ref:`rotations`) + existing (:obj:`bool`): If the agent exists in the world or not (deprecated) + """ + + _type_keys = { + "SphereAgent": SphereAgent, + "UavAgent": UavAgent, + "NavAgent": NavAgent, + "AndroidAgent": AndroidAgent, + "HandAgent": HandAgent, + "TurtleAgent": TurtleAgent, + "HoveringAUV": HoveringAUV, + "TorpedoAUV": TorpedoAUV, + "SurfaceVessel": SurfaceVessel, + } + + def __init__(self, agent_name, agent_type, sensors=None, starting_loc=(0, 0, 0), + starting_rot=(0, 0, 0), existing=False, is_main_agent=False): + self.starting_loc = starting_loc + self.starting_rot = starting_rot + self.existing = existing + self.sensors = sensors or list() + self.is_main_agent = is_main_agent + for i, sensor_def in enumerate(self.sensors): + if not isinstance(sensor_def, SensorDefinition): + self.sensors[i] = \ + SensorDefinition(agent_name, agent_type, + sensor_def.sensor_type, sensor_def) + self.name = agent_name + + if isinstance(agent_type, str): + self.type = AgentDefinition._type_keys[agent_type] + else: + self.type = agent_type + + +class AgentFactory: + """Creates an agent object + """ + + @staticmethod + def build_agent(client, agent_def): + """Constructs an agent + + Args: + client (:class:`holoocean.holooceanclient.HoloOceanClient`): HoloOceanClient agent is + associated with + agent_def (:class:`AgentDefinition`): Definition of the agent to instantiate + + Returns: + + """ + return agent_def.type(client, agent_def.name) diff --git a/PythonAPI/carla/source/holoocean/command.py b/PythonAPI/carla/source/holoocean/command.py new file mode 100644 index 0000000000..55367ef33b --- /dev/null +++ b/PythonAPI/carla/source/holoocean/command.py @@ -0,0 +1,452 @@ +"""This module contains the classes used for formatting and sending commands to the HoloOcean +backend. Most of these commands are just used internally by HoloOcean, regular users do not need to +worry about these. + +""" + + +import numpy as np +from holoocean.exceptions import HoloOceanException + +class CommandsGroup: + """Represents a list of commands + + Can convert list of commands to json. + + """ + + def __init__(self): + self._commands = [] + + def add_command(self, command): + """Adds a command to the list + + Args: + command (:class:`Command`): A command to add.""" + self._commands.append(command) + + def to_json(self): + """ + Returns: + :obj:`str`: Json for commands array object and all of the commands inside the array. + + """ + commands = ",".join(map(lambda x: x.to_json(), self._commands)) + return "{\"commands\": [" + commands + "]}" + + def clear(self): + """Clear the list of commands. + + """ + self._commands.clear() + + @property + def size(self): + """ + Returns: + int: Size of commands group""" + return len(self._commands) + + +class Command: + """Base class for Command objects. + + Commands are used for IPC between the holoocean python bindings and holoocean + binaries. + + Derived classes must set the ``_command_type``. + + The order in which :meth:`add_number_parameters` and :meth:`add_number_parameters` are called + is significant, they are added to an ordered list. Ensure that you are adding parameters in + the order the client expects them. + + """ + + def __init__(self): + self._parameters = [] + self._command_type = "" + + def set_command_type(self, command_type): + """Set the type of the command. + + Args: + command_type (:obj:`str`): This is the name of the command that it will be set to. + + """ + self._command_type = command_type + + def add_number_parameters(self, number): + """Add given number parameters to the internal list. + + Args: + number (:obj:`list` of :obj:`int`/:obj:`float`, or singular :obj:`int`/:obj:`float`): + A number or list of numbers to add to the parameters. + + """ + if isinstance(number, list) or isinstance(number, tuple): + for x in number: + self.add_number_parameters(x) + return + self._parameters.append("{ \"value\": " + str(number) + " }") + + def add_string_parameters(self, string): + """Add given string parameters to the internal list. + + Args: + string (:obj:`list` of :obj:`str` or :obj:`str`): + A string or list of strings to add to the parameters. + + """ + if isinstance(string, list) or isinstance(string, tuple): + for x in string: + self.add_string_parameters(x) + return + self._parameters.append("{ \"value\": \"" + string + "\" }") + + def to_json(self): + """Converts to json. + + Returns: + :obj:`str`: This object as a json string. + + """ + to_return = "{ \"type\": \"" + self._command_type +\ + "\", \"params\": [" + ",".join(self._parameters) + "]}" + return to_return + + +class CommandCenter: + """Manages pending commands to send to the client (the engine). + + Args: + client (:class:`~holoocean.holooceanclient.HoloOceanClient`): Client to send commands to + + """ + def __init__(self, client): + self._client = client + + # Set up command buffer + self._command_bool_ptr = self._client.malloc("command_bool", [1], np.bool_) + # This is the size of the command buffer that HoloOcean expects/will read. + self.max_buffer = 1048576 + self._command_buffer_ptr = self._client.malloc("command_buffer", [self.max_buffer], np.byte) + self._commands = CommandsGroup() + self._should_write_to_command_buffer = False + + def clear(self): + """Clears pending commands + + """ + self._commands.clear() + + def handle_buffer(self): + """Writes the list of commands into the command buffer, if needed. + + Checks if we should write to the command buffer, writes all of the queued commands to the + buffer, and then clears the contents of the self._commands list + + """ + if self._should_write_to_command_buffer: + self._write_to_command_buffer(self._commands.to_json()) + self._should_write_to_command_buffer = False + self._commands.clear() + + def enqueue_command(self, command_to_send): + """Adds command to outgoing queue. + + Args: + command_to_send (:class:`Command`): Command to add to queue + + """ + self._should_write_to_command_buffer = True + self._commands.add_command(command_to_send) + + def _write_to_command_buffer(self, to_write): + """Write input to the command buffer. + + Reformat input string to the correct format. + + Args: + to_write (:class:`str`): The string to write to the command buffer. + + """ + np.copyto(self._command_bool_ptr, True) + to_write += '0' # The gason JSON parser in holoocean expects a 0 at the end of the file. + input_bytes = str.encode(to_write) + if len(input_bytes) > self.max_buffer: + raise HoloOceanException("Error: Command length exceeds buffer size") + for index, val in enumerate(input_bytes): + self._command_buffer_ptr[index] = val + + @property + def queue_size(self): + """ + Returns: + int: Size of commands queue""" + return self._commands.size + + +class SpawnAgentCommand(Command): + """Spawn an agent in the world. + + Args: + location (:obj:`list` of :obj:`float`): ``[x, y, z]`` location to spawn agent (see :ref:`coordinate-system`) + name (:obj:`str`): The name of the agent. + agent_type (:obj:`str` or type): The type of agent to spawn (UAVAgent, NavAgent, ...) + + """ + + def __init__(self, location, rotation, name, agent_type, is_main_agent=False): + super(SpawnAgentCommand, self).__init__() + self._command_type = "SpawnAgent" + self.set_location(location) + self.set_rotation(rotation) + self.set_type(agent_type) + self.set_name(name) + self.add_number_parameters(int(is_main_agent)) + + def set_location(self, location): + """Set where agent will be spawned. + + Args: + location (:obj:`list` of :obj:`float`): ``[x, y, z]`` location to spawn agent (see :ref:`coordinate-system`) + + """ + if len(location) != 3: + raise HoloOceanException("Invalid location given to spawn agent command") + self.add_number_parameters(location) + + def set_rotation(self, rotation): + """Set where agent will be spawned. + + Args: + rotation (:obj:`list` of :obj:`float`): ``[roll, pitch, yaw]`` rotation for agent. + (see :ref:`rotations`) + + """ + if len(rotation) != 3: + raise HoloOceanException("Invalid rotation given to spawn agent command") + self.add_number_parameters(rotation) + + def set_name(self, name): + """Set agents name + + Args: + name (:obj:`str`): The name to set the agent to. + + """ + self.add_string_parameters(name) + + def set_type(self, agent_type): + """Set the type of agent. + + Args: + agent_type (:obj:`str` or :obj:`type`): The type of agent to spawn. + + """ + if not isinstance(agent_type, str): + agent_type = agent_type.agent_type # Get str from type + self.add_string_parameters(agent_type) + + +class DebugDrawCommand(Command): + """Draw debug geometry in the world. + + Args: + draw_type (:obj:`int`) : The type of object to draw + + - ``0``: line + - ``1``: arrow + - ``2``: box + - ``3``: point + + start (:obj:`list` of :obj:`float`): The start ``[x, y, z]`` location in meters of the object. + (see :ref:`coordinate-system`) + end (:obj:`list` of :obj:`float`): The end ``[x, y, z]`` location in meters of the object + (not used for point, and extent for box) + color (:obj:`list` of :obj:`float`): ``[r, g, b]`` color value (from 0 to 255). + thickness (:obj:`float`): thickness of the line/object + lifetime (:obj:`float`): Number of simulation seconds the object should persist. If 0, makes persistent + + """ + def __init__(self, draw_type, start, end, color, thickness, lifetime): + super(DebugDrawCommand, self).__init__() + self._command_type = "DebugDraw" + + self.add_number_parameters(draw_type) + self.add_number_parameters(start) + self.add_number_parameters(end) + self.add_number_parameters(color) + self.add_number_parameters(thickness) + self.add_number_parameters(lifetime) + + +class TeleportCameraCommand(Command): + """Move the viewport camera (agent follower) + + Args: + location (:obj:`list` of :obj:`float`): The ``[x, y, z]`` location to give the camera + (see :ref:`coordinate-system`) + rotation (:obj:`list` of :obj:`float`): The ``[roll, pitch, yaw]`` rotation to give the camera + (see :ref:`rotations`) + + """ + def __init__(self, location, rotation): + Command.__init__(self) + self._command_type = "TeleportCamera" + self.add_number_parameters(location) + self.add_number_parameters(rotation) + + +class AddSensorCommand(Command): + """Add a sensor to an agent + + Args: + sensor_definition (~holoocean.sensors.SensorDefinition): Sensor to add + """ + + def __init__(self, sensor_definition): + Command.__init__(self) + self._command_type = "AddSensor" + self.add_string_parameters(sensor_definition.agent_name) + self.add_string_parameters(sensor_definition.sensor_name) + self.add_string_parameters(sensor_definition.type.sensor_type) + self.add_string_parameters(sensor_definition.get_config_json_string()) + self.add_string_parameters(sensor_definition.socket) + + self.add_number_parameters(sensor_definition.location[0]) + self.add_number_parameters(sensor_definition.location[1]) + self.add_number_parameters(sensor_definition.location[2]) + + self.add_number_parameters(sensor_definition.rotation[0]) + self.add_number_parameters(sensor_definition.rotation[1]) + self.add_number_parameters(sensor_definition.rotation[2]) + + +class RemoveSensorCommand(Command): + """Remove a sensor from an agent + + Args: + agent (:obj:`str`): Name of agent to modify + sensor (:obj:`str`): Name of the sensor to remove + + """ + def __init__(self, agent, sensor): + Command.__init__(self) + self._command_type = "RemoveSensor" + self.add_string_parameters(agent) + self.add_string_parameters(sensor) + +class RotateSensorCommand(Command): + """Rotate a sensor on the agent + + Args: + agent (:obj:`str`): Name of agent + sensor (:obj:`str`): Name of the sensor to rotate + rotation (:obj:`list` of :obj:`float`): ``[roll, pitch, yaw]`` rotation for sensor. + + """ + def __init__(self, agent, sensor, rotation): + Command.__init__(self) + self._command_type = "RotateSensor" + self.add_string_parameters(agent) + self.add_string_parameters(sensor) + self.add_number_parameters(rotation) + + +class RenderViewportCommand(Command): + """Enable or disable the viewport. Note that this does not prevent the viewport from being shown, + it just prevents it from being updated. + + Args: + render_viewport (:obj:`bool`): If viewport should be rendered + + """ + def __init__(self, render_viewport): + Command.__init__(self) + self.set_command_type("RenderViewport") + self.add_number_parameters(int(bool(render_viewport))) + + +class RGBCameraRateCommand(Command): + """Set the number of ticks between captures of the RGB camera. + + Args: + agent_name (:obj:`str`): name of the agent to modify + sensor_name (:obj:`str`): name of the sensor to modify + ticks_per_capture (:obj:`int`): number of ticks between captures + + """ + def __init__(self, agent_name, sensor_name, ticks_per_capture): + Command.__init__(self) + self._command_type = "RGBCameraRate" + self.add_string_parameters(agent_name) + self.add_string_parameters(sensor_name) + self.add_number_parameters(ticks_per_capture) + + +class RenderQualityCommand(Command): + """Adjust the rendering quality of HoloOcean + + Args: + render_quality (int): 0 = low, 1 = medium, 3 = high, 3 = epic + + """ + def __init__(self, render_quality): + Command.__init__(self) + self.set_command_type("AdjustRenderQuality") + self.add_number_parameters(int(render_quality)) + + +class CustomCommand(Command): + """Send a custom command to the currently loaded world. + + Args: + name (:obj:`str`): The name of the command, ex "OpenDoor" + num_params (obj:`list` of :obj:`int`): List of arbitrary number parameters + string_params (obj:`list` of :obj:`int`): List of arbitrary string parameters + + """ + def __init__(self, name, num_params=None, string_params=None): + if num_params is None: + num_params = [] + + if string_params is None: + string_params = [] + + Command.__init__(self) + self.set_command_type("CustomCommand") + self.add_string_parameters(name) + self.add_number_parameters(num_params) + self.add_string_parameters(string_params) + + +######################## HOLOOCEAN CUSTOM COMMANDS ########################### + +class SendAcousticMessageCommand(Command): + """Set the number of ticks between captures of the RGB camera. + + Args: + agent_name (:obj:`str`): name of the agent to modify + sensor_name (:obj:`str`): name of the sensor to modify + num (:obj:`int`): number of ticks between captures + + """ + def __init__(self, from_agent_name, from_sensor_name, to_agent_name, to_sensor_name): + Command.__init__(self) + self._command_type = "SendAcousticMessage" + self.add_string_parameters(from_agent_name) + self.add_string_parameters(from_sensor_name) + self.add_string_parameters(to_agent_name) + self.add_string_parameters(to_sensor_name) + +class SendOpticalMessageCommand(Command): + """Send information through OpticalModem. + """ + def __init__(self, from_agent_name, from_sensor_name, to_agent_name, to_sensor_name): + Command.__init__(self) + self._command_type = "SendOpticalMessage" + self.add_string_parameters(from_agent_name) + self.add_string_parameters(from_sensor_name) + self.add_string_parameters(to_agent_name) + self.add_string_parameters(to_sensor_name) \ No newline at end of file diff --git a/PythonAPI/carla/source/holoocean/environments.py b/PythonAPI/carla/source/holoocean/environments.py new file mode 100644 index 0000000000..a25fc200cb --- /dev/null +++ b/PythonAPI/carla/source/holoocean/environments.py @@ -0,0 +1,1091 @@ +"""Module containing the environment interface for HoloOcean. +An environment contains all elements required to communicate with a world binary or HoloOceanCore +editor. + +It specifies an environment, which contains a number of agents, and the interface for communicating +with the agents. +""" +import atexit +import os +import random +import subprocess +import sys + +import numpy as np + +from holoocean.command import ( + CommandCenter, + SpawnAgentCommand, + TeleportCameraCommand, + RenderViewportCommand, + RenderQualityCommand, + CustomCommand, + DebugDrawCommand, +) + +from holoocean.exceptions import HoloOceanException +from holoocean.holooceanclient import HoloOceanClient +from holoocean.agents import AgentDefinition, SensorDefinition, AgentFactory +from holoocean.weather import WeatherController + +from holoocean.sensors import AcousticBeaconSensor +from holoocean.sensors import OpticalModemSensor + + +class HoloOceanEnvironment: + """Proxy for communicating with a HoloOcean world + + Instantiate this object using :meth:`holoocean.holoocean.make`. + + Args: + agent_definitions (:obj:`list` of :class:`AgentDefinition`): + Which agents are already in the environment + + binary_path (:obj:`str`, optional): + The path to the binary to load the world from. Defaults to None. + + window_size ((:obj:`int`,:obj:`int`)): + height, width of the window to open + + start_world (:obj:`bool`, optional): + Whether to load a binary or not. Defaults to True. + + uuid (:obj:`str`): + A unique identifier, used when running multiple instances of holoocean. Defaults to "". + + gl_version (:obj:`int`, optional): + The version of OpenGL to use for Linux. Defaults to 4. + + verbose (:obj:`bool`): + If engine log output should be printed to stdout + + pre_start_steps (:obj:`int`): + Number of ticks to call after initializing the world, allows the level to load and settle. + + show_viewport (:obj:`bool`, optional): + If the viewport should be shown (Linux only) Defaults to True. + + ticks_per_sec (:obj:`int`, optional): + The number of frame ticks per unreal seconds. This will override whatever is + in the configuration json. Defaults to 30. + + frames_per_sec (:obj:`int` or :obj:`bool`, optional): + The max number of frames ticks per real seconds. This will override whatever is + in the configuration json. If True, will match ticks_per_sec. If False, will not be + turned on. If an integer, will set to that value. Defaults to true. + + copy_state (:obj:`bool`, optional): + If the state should be copied or returned as a reference. Defaults to True. + + scenario (:obj:`dict`): + The scenario that is to be loaded. See :ref:`scenario-files` for the schema. + + """ + + def __init__( + self, + agent_definitions=None, + binary_path=None, + window_size=None, + start_world=True, + uuid="", + gl_version=4, + verbose=False, + pre_start_steps=2, + show_viewport=True, + ticks_per_sec=None, + frames_per_sec=None, + copy_state=True, + scenario=None, + ): + if agent_definitions is None: + agent_definitions = [] + + # Initialize variables + + if window_size is None: + # Check if it has been configured in the scenario + if scenario is not None and "window_height" in scenario: + self._window_size = scenario["window_height"], scenario["window_width"] + else: + # Default resolution + self._window_size = 720, 1280 + else: + self._window_size = window_size + + # Use env size from scenario/world config + if scenario is not None and "env_min" in scenario: + self._env_min = scenario["env_min"] + self._env_max = scenario["env_max"] + # Default resolution + else: + self._env_min = [-10, -10, -10] + self._env_max = [10, 10, 10] + + if scenario is not None and "octree_min" in scenario: + self._octree_min = scenario["octree_min"] + self._octree_max = scenario["octree_max"] + else: + # Default resolution + self._octree_min = 0.02 + self._octree_max = 5 + + if scenario is not None and "lcm_provider" not in scenario: + scenario["lcm_provider"] = "" + + self._uuid = uuid + self._pre_start_steps = pre_start_steps + self._copy_state = copy_state + self._scenario = scenario + self._initial_agent_defs = agent_definitions + self._spawned_agent_defs = [] + + # Choose one that was passed in function + if ticks_per_sec is not None: + self._ticks_per_sec = ticks_per_sec + # otherwise use one in scenario + elif "ticks_per_sec" in scenario: + self._ticks_per_sec = scenario["ticks_per_sec"] + # default to 30 + else: + self._ticks_per_sec = 30 + + # If one wasn't passed in, use one in scenario + if frames_per_sec is None and "frames_per_sec" in scenario: + frames_per_sec = scenario["frames_per_sec"] + # default to true + else: + frames_per_sec = True + + # parse frames_per_sec + if frames_per_sec is True: + self._frames_per_sec = self._ticks_per_sec + elif frames_per_sec is False: + self._frames_per_sec = 0 + else: + self._frames_per_sec = frames_per_sec + + self._lcm = None + self._num_ticks = 0 + + # Start world based on OS + if start_world: + world_key = self._scenario["world"] + if os.name == "posix": + self.__linux_start_process__( + binary_path, + world_key, + gl_version, + verbose=verbose, + show_viewport=show_viewport, + ) + elif os.name == "nt": + self.__windows_start_process__( + binary_path, world_key, verbose=verbose, show_viewport=show_viewport + ) + else: + raise HoloOceanException("Unknown platform: " + os.name) + + # Initialize Client + self._client = HoloOceanClient(self._uuid) + self._command_center = CommandCenter(self._client) + self._client.command_center = self._command_center + self._reset_ptr = self._client.malloc("RESET", [1], np.bool_) + self._reset_ptr[0] = False + + # Initialize environment controller + self.weather = WeatherController(self.send_world_command) + + # Set up agents already in the world + self.agents = dict() + self._state_dict = dict() + self._agent = None + + # Set the default state function + self.num_agents = len(self.agents) + + # Whether we need to wait for a sonar to load + self.start_world = start_world + self._has_sonar = False + + if self.num_agents == 1: + self._default_state_fn = self._get_single_state + else: + self._default_state_fn = self._get_full_state + + self._client.acquire(self._timeout) + + if os.name == "posix" and show_viewport is False: + self.should_render_viewport(False) + + # Flag indicates if the user has called .reset() before .tick() and .step() + self._initial_reset = False + self.reset() + + @property + def _timeout(self): + # Returns the timeout that should be processed + # Turns off timeout when creating octrees might be made + if self._has_sonar or not self.start_world: + if os.name == "posix": + return None + elif os.name == "nt": + import win32event + + return win32event.INFINITE + + else: + return 60 + + @property + def action_space(self): + """Gives the action space for the main agent. + + Returns: + :class:`~holoocean.spaces.ActionSpace`: The action space for the main agent. + """ + return self._agent.action_space + + def info(self): + """Returns a string with specific information about the environment. + This information includes which agents are in the environment and which sensors they have. + + Returns: + :obj:`str`: Information in a string format. + """ + result = list() + result.append("Agents:\n") + for agent_name in self.agents: + agent = self.agents[agent_name] + result.append("\tName: ") + result.append(agent.name) + result.append("\n\tType: ") + result.append(type(agent).__name__) + result.append("\n\t") + result.append("Sensors:\n") + for _, sensor in agent.sensors.items(): + result.append("\t\t") + result.append(sensor.name) + result.append("\n") + return "".join(result) + + def _load_scenario(self): + """Loads the scenario defined in self._scenario_key. + + Instantiates agents, sensors, and weather. + + If no scenario is defined, does nothing. + """ + if self._scenario is None: + return + + for agent in self._scenario["agents"]: + sensors = [] + for sensor in agent["sensors"]: + if "sensor_type" not in sensor: + raise HoloOceanException( + "Sensor for agent {} is missing required key " + "'sensor_type'".format(agent["agent_name"]) + ) + + # Default values for a sensor + sensor_config = { + "location": [0, 0, 0], + "rotation": [0, 0, 0], + "socket": "", + "configuration": None, + "sensor_name": sensor["sensor_type"], + "existing": False, + "Hz": self._ticks_per_sec, + "lcm_channel": None, + } + # Overwrite the default values with what is defined in the scenario config + sensor_config.update(sensor) + + # set up sensor rates + if self._ticks_per_sec < sensor_config["Hz"]: + raise ValueError( + f"{sensor_config['sensor_name']} is sampled at {sensor_config['Hz']} which is greater than ticks_per_sec {self._ticks_per_sec}" + ) + + # round sensor rate as needed + tick_every = self._ticks_per_sec / sensor_config["Hz"] + if int(tick_every) != tick_every: + print( + f"{sensor_config['sensor_name']} rate {sensor_config['Hz']} is not a factor of ticks_per_sec {self._ticks_per_sec}, rounding to {self._ticks_per_sec//int(tick_every)}" + ) + sensor_config["tick_every"] = int(tick_every) + + sensors.append( + SensorDefinition( + agent["agent_name"], + agent["agent_type"], + sensor_config["sensor_name"], + sensor_config["sensor_type"], + socket=sensor_config["socket"], + location=sensor_config["location"], + rotation=sensor_config["rotation"], + config=sensor_config["configuration"], + tick_every=sensor_config["tick_every"], + lcm_channel=sensor_config["lcm_channel"], + ) + ) + + if sensor_config["sensor_type"] in SensorDefinition._sonar_sensors: + self._has_sonar = True + + # Import LCM if needed + if sensor_config["lcm_channel"] is not None and self._lcm is None: + globals()["lcm"] = __import__("lcm") + self._lcm = lcm.LCM(self._scenario["lcm_provider"]) + + # Default values for an agent + agent_config = { + "location": [0, 0, 0], + "rotation": [0, 0, 0], + "agent_name": agent["agent_type"], + "existing": False, + "location_randomization": [0, 0, 0], + "rotation_randomization": [0, 0, 0], + } + + agent_config.update(agent) + is_main_agent = False + + if "main_agent" in self._scenario: + is_main_agent = self._scenario["main_agent"] == agent["agent_name"] + + agent_location = agent_config["location"] + agent_rotation = agent_config["rotation"] + + # Randomize the agent start location + dx = agent_config["location_randomization"][0] + dy = agent_config["location_randomization"][1] + dz = agent_config["location_randomization"][2] + + agent_location[0] += random.uniform(-dx, dx) + agent_location[1] += random.uniform(-dy, dy) + agent_location[2] += random.uniform(-dz, dz) + + # Randomize the agent rotation + d_pitch = agent_config["rotation_randomization"][0] + d_roll = agent_config["rotation_randomization"][1] + d_yaw = agent_config["rotation_randomization"][2] + + agent_rotation[0] += random.uniform(-d_pitch, d_pitch) + agent_rotation[1] += random.uniform(-d_roll, d_roll) + agent_rotation[2] += random.uniform(-d_yaw, d_yaw) + + agent_def = AgentDefinition( + agent_config["agent_name"], + agent_config["agent_type"], + starting_loc=agent_location, + starting_rot=agent_rotation, + sensors=sensors, + existing=agent_config["existing"], + is_main_agent=is_main_agent, + ) + + self.add_agent(agent_def, is_main_agent) + self.agents[agent["agent_name"]].set_control_scheme(agent["control_scheme"]) + self._spawned_agent_defs.append(agent_def) + + if "weather" in self._scenario: + weather = self._scenario["weather"] + if "hour" in weather: + self.weather.set_day_time(weather["hour"]) + if "type" in weather: + self.weather.set_weather(weather["type"]) + if "fog_density" in weather: + self.weather.set_fog_density(weather["fog_density"]) + if "day_cycle_length" in weather: + day_cycle_length = weather["day_cycle_length"] + self.weather.start_day_cycle(day_cycle_length) + + def reset(self): + """Resets the environment, and returns the state. + If it is a single agent environment, it returns that state for that agent. Otherwise, it + returns a dict from agent name to state. + + Returns: + :obj:`tuple` or :obj:`dict`: + Returns the same as `tick`. + """ + # Reset level + self._initial_reset = True + self._reset_ptr[0] = True + for agent in self.agents.values(): + agent.clear_action() + + # reset all sensors + for agent_name, agent in self.agents.items(): + for sensor_name, sensor in agent.sensors.items(): + sensor.reset() + + self.tick( + publish=False + ) # Must tick once to send reset before sending spawning commands + self.tick( + publish=False + ) # Bad fix to potential race condition. See issue BYU-PCCL/holodeck#224 + self.tick(publish=False) + # Clear command queue + if self._command_center.queue_size > 0: + print( + "Warning: Reset called before all commands could be sent. Discarding", + self._command_center.queue_size, + "commands.", + ) + self._command_center.clear() + + # Load agents + self._spawned_agent_defs = [] + self.agents = dict() + self._state_dict = dict() + for agent_def in self._initial_agent_defs: + self.add_agent(agent_def, agent_def.is_main_agent) + + self._load_scenario() + + self.num_agents = len(self.agents) + + if self.num_agents == 1: + self._default_state_fn = self._get_single_state + else: + self._default_state_fn = self._get_full_state + + for _ in range(self._pre_start_steps + 1): + self.tick(publish=False) + + return self._default_state_fn() + + def step(self, action, ticks=1, publish=True): + """Supplies an action to the main agent and tells the environment to tick once. + Primary mode of interaction for single agent environments. + + Args: + action (:obj:`np.ndarray`): An action for the main agent to carry out on the next tick. + ticks (:obj:`int`): Number of times to step the environment with this action. + If ticks > 1, this function returns the last state generated. + publish (:obj:`bool`): Whether or not to publish as defined by scenario. Defaults to True. + + Returns: + :obj:`dict`: + A dictionary from agent name to its full state. The full state is another + dictionary from :obj:`holoocean.sensors.Sensors` enum to np.ndarray, containing the + sensors information for each sensor. The sensors always include the reward and + terminal sensors. Reward and terminals can also be gotten through :meth:`get_reward_terminal`. + + Will return the state from the last tick executed. + """ + if not self._initial_reset: + raise HoloOceanException("You must call .reset() before .step()") + + for _ in range(ticks): + if self._agent is not None: + self._agent.act(action) + + self._command_center.handle_buffer() + self._client.release() + self._client.acquire(self._timeout) + + last_state = self._default_state_fn() + + self._tick_sensor() + self._num_ticks += 1 + + if publish and self._lcm is not None: + self._publish(last_state) + + return last_state + + def act(self, agent_name, action): + """Supplies an action to a particular agent, but doesn't tick the environment. + Primary mode of interaction for multi-agent environments. After all agent commands are + supplied, they can be applied with a call to `tick`. + + Args: + agent_name (:obj:`str`): The name of the agent to supply an action for. + action (:obj:`np.ndarray` or :obj:`list`): The action to apply to the agent. This + action will be applied every time `tick` is called, until a new action is supplied + with another call to act. + """ + self.agents[agent_name].act(action) + + def get_joint_constraints(self, agent_name, joint_name): + """Returns the corresponding swing1, swing2 and twist limit values for the + specified agent and joint. Will return None if the joint does not exist for the agent. + + Returns: + :obj:`np.ndarray` + """ + return self.agents[agent_name].get_joint_constraints(joint_name) + + def tick(self, num_ticks=1, publish=True): + """Ticks the environment once. Normally used for multi-agent environments. + + Args: + num_ticks (:obj:`int`): Number of ticks to perform. Defaults to 1. + publish (:obj:`bool`): Whether or not to publish as defined by scenario. Defaults to True. + + Returns: + :obj:`dict`: + A dictionary from agent name to its full state. The full state is another + dictionary from :obj:`holoocean.sensors.Sensors` enum to np.ndarray, containing the + sensors information for each sensor. The sensors always include the reward and + terminal sensors. Reward and terminals can also be gotten through :meth:`get_reward_terminal`. + + Will return the state from the last tick executed. + """ + if not self._initial_reset: + raise HoloOceanException("You must call .reset() before .tick()") + + for _ in range(num_ticks): + self._command_center.handle_buffer() + + self._client.release() + self._client.acquire(self._timeout) + + state = self._default_state_fn() + + self._tick_sensor() + self._num_ticks += 1 + + if publish and self._lcm is not None: + self._publish(state) + + return state + + def _publish(self, state): + """Publishes given state to channels chosen by the scenario config.""" + # if it was a partial state + if self._agent.name not in state: + state = {self._agent.name: state} + + # iterate through all agents and sensors + for agent_name, agent in self.agents.items(): + for sensor_name, sensor in agent.sensors.items(): + # check if it's a full state, or single one + if sensor.lcm_msg is not None and sensor_name in state[agent_name]: + # send message if it's in the dictionary and if LCM message is turned on + sensor.lcm_msg.set_value( + int(1000 * self._num_ticks / self._ticks_per_sec), + state[agent_name][sensor_name], + ) + self._lcm.publish( + sensor.lcm_msg.channel, sensor.lcm_msg.msg.encode() + ) + + def _enqueue_command(self, command_to_send): + self._command_center.enqueue_command(command_to_send) + + def add_agent(self, agent_def, is_main_agent=False): + """Add an agent in the world. + + It will be spawn when :meth:`tick` or :meth:`step` is called next. + + The agent won't be able to be used until the next frame. + + Args: + agent_def (:class:`~holoocean.agents.AgentDefinition`): The definition of the agent to + spawn. + """ + if agent_def.name in self.agents: + raise HoloOceanException("Error. Duplicate agent name. ") + + self.agents[agent_def.name] = AgentFactory.build_agent(self._client, agent_def) + self._state_dict[agent_def.name] = self.agents[agent_def.name].agent_state_dict + + if not agent_def.existing: + command_to_send = SpawnAgentCommand( + location=agent_def.starting_loc, + rotation=agent_def.starting_rot, + name=agent_def.name, + agent_type=agent_def.type.agent_type, + is_main_agent=agent_def.is_main_agent, + ) + + self._client.command_center.enqueue_command(command_to_send) + self.agents[agent_def.name].add_sensors(agent_def.sensors) + if is_main_agent: + self._agent = self.agents[agent_def.name] + + def spawn_prop( + self, + prop_type, + location=None, + rotation=None, + scale=1, + sim_physics=False, + material="", + tag="", + ): + """Spawns a basic prop object in the world like a box or sphere. + + Prop will not persist after environment reset. + + Args: + prop_type (:obj:`string`): + The type of prop to spawn. Can be ``box``, ``sphere``, ``cylinder``, or ``cone``. + + location (:obj:`list` of :obj:`float`): + The ``[x, y, z]`` location of the prop. + + rotation (:obj:`list` of :obj:`float`): + The ``[roll, pitch, yaw]`` rotation of the prop. + + scale (:obj:`list` of :obj:`float`) or (:obj:`float`): + The ``[x, y, z]`` scalars to the prop size, where the default size is 1 meter. + If given a single float value, then every dimension will be scaled to that value. + + sim_physics (:obj:`boolean`): + Whether the object is mobile and is affected by gravity. + + material (:obj:`string`): + The type of material (texture) to apply to the prop. Can be ``white``, ``gold``, + ``cobblestone``, ``brick``, ``wood``, ``grass``, ``steel``, or ``black``. If left + empty, the prop will have the a simple checkered gray material. + + tag (:obj:`string`): + The tag to apply to the prop. Useful for task references. + """ + location = [0, 0, 0] if location is None else location + rotation = [0, 0, 0] if rotation is None else rotation + # if the given scale is an single value, then scale every dimension to that value + if not isinstance(scale, list): + scale = [scale, scale, scale] + sim_physics = 1 if sim_physics else 0 + + prop_type = prop_type.lower() + material = material.lower() + + available_props = ["box", "sphere", "cylinder", "cone"] + available_materials = [ + "white", + "gold", + "cobblestone", + "brick", + "wood", + "grass", + "steel", + "black", + ] + + if prop_type not in available_props: + raise HoloOceanException( + "{} not an available prop. Available prop types: {}".format( + prop_type, available_props + ) + ) + if material not in available_materials and material != "": + raise HoloOceanException( + "{} not an available material. Available material types: {}".format( + material, available_materials + ) + ) + + self.send_world_command( + "SpawnProp", + num_params=[location, rotation, scale, sim_physics], + string_params=[prop_type, material, tag], + ) + + def draw_line(self, start, end, color=None, thickness=10.0, lifetime=1.0): + """Draws a debug line in the world + + Args: + start (:obj:`list` of :obj:`float`): The start ``[x, y, z]`` location in meters of the line. + (see :ref:`coordinate-system`) + end (:obj:`list` of :obj:`float`): The end ``[x, y, z]`` location in meters of the line + color (:obj:`list``): ``[r, g, b]`` color value (from 0 to 255). Defaults to [255, 0, 0] (red). + thickness (:obj:`float`): Thickness of the line. Defaults to 10. + lifetime (:obj:`float`): Number of simulation seconds the object should persist. If 0, makes persistent. Defaults to 1. + """ + color = [255, 0, 0] if color is None else color + command_to_send = DebugDrawCommand(0, start, end, color, thickness, lifetime) + self._enqueue_command(command_to_send) + + def draw_arrow(self, start, end, color=None, thickness=10.0, lifetime=1.0): + """Draws a debug arrow in the world + + Args: + start (:obj:`list` of :obj:`float`): The start ``[x, y, z]`` location in meters of the line. + (see :ref:`coordinate-system`) + end (:obj:`list` of :obj:`float`): The end ``[x, y, z]`` location in meters of the arrow + color (:obj:`list`): ``[r, g, b]`` color value (from 0 to 255). Defaults to [255, 0, 0] (red). + thickness (:obj:`float`): Thickness of the arrow. Defaults to 10. + lifetime (:obj:`float`): Number of simulation seconds the object should persist. If 0, makes persistent. Defaults to 1. + """ + color = [255, 0, 0] if color is None else color + command_to_send = DebugDrawCommand(1, start, end, color, thickness, lifetime) + self._enqueue_command(command_to_send) + + def draw_box(self, center, extent, color=None, thickness=10.0, lifetime=1.0): + """Draws a debug box in the world + + Args: + center (:obj:`list` of :obj:`float`): The start ``[x, y, z]`` location in meters of the box. + (see :ref:`coordinate-system`) + extent (:obj:`list` of :obj:`float`): The ``[x, y, z]`` extent of the box + color (:obj:`list`): ``[r, g, b]`` color value (from 0 to 255). Defaults to [255, 0, 0] (red). + thickness (:obj:`float`): Thickness of the lines. Defaults to 10. + lifetime (:obj:`float`): Number of simulation seconds the object should persist. If 0, makes persistent. Defaults to 1. + """ + color = [255, 0, 0] if color is None else color + command_to_send = DebugDrawCommand( + 2, center, extent, color, thickness, lifetime + ) + self._enqueue_command(command_to_send) + + def draw_point(self, loc, color=None, thickness=10.0, lifetime=1.0): + """Draws a debug point in the world + + Args: + loc (:obj:`list` of :obj:`float`): The ``[x, y, z]`` start of the box. + (see :ref:`coordinate-system`) + color (:obj:`list` of :obj:`float`): ``[r, g, b]`` color value (from 0 to 255). Defaults to [255, 0, 0] (red). + thickness (:obj:`float`): Thickness of the point. Defaults to 10. + lifetime (:obj:`float`): Number of simulation seconds the object should persist. If 0, makes persistent. Defaults to 1. + """ + color = [255, 0, 0] if color is None else color + command_to_send = DebugDrawCommand( + 3, loc, [0, 0, 0], color, thickness, lifetime + ) + self._enqueue_command(command_to_send) + + def move_viewport(self, location, rotation): + """Teleport the camera to the given location + + By the next tick, the camera's location and rotation will be updated + + Args: + location (:obj:`list` of :obj:`float`): The ``[x, y, z]`` location to give the camera + (see :ref:`coordinate-system`) + rotation (:obj:`list` of :obj:`float`): The x-axis that the camera should look down. Other 2 axes are formed + by a horizontal y-aixs, and then the corresponding z-axis. + (see :ref:`rotations`) + + """ + # test_viewport_capture_after_teleport + self._enqueue_command(TeleportCameraCommand(location, rotation)) + + def should_render_viewport(self, render_viewport): + """Controls whether the viewport is rendered or not + + Args: + render_viewport (:obj:`boolean`): If the viewport should be rendered + """ + self._enqueue_command(RenderViewportCommand(render_viewport)) + + def set_render_quality(self, render_quality): + """Adjusts the rendering quality of HoloOcean. + + Args: + render_quality (:obj:`int`): An integer between 0 = Low Quality and 3 = Epic quality. + """ + self._enqueue_command(RenderQualityCommand(render_quality)) + + def set_control_scheme(self, agent_name, control_scheme): + """Set the control scheme for a specific agent. + + Args: + agent_name (:obj:`str`): The name of the agent to set the control scheme for. + control_scheme (:obj:`int`): A control scheme value + (see :class:`~holoocean.agents.ControlSchemes`) + """ + if agent_name not in self.agents: + print("No such agent %s" % agent_name) + else: + self.agents[agent_name].set_control_scheme(control_scheme) + + def send_world_command(self, name, num_params=None, string_params=None): + """Send a world command. + + A world command sends an abitrary command that may only exist in a specific world or + package. It is given a name and any amount of string and number parameters that allow it to + alter the state of the world. + + If a command is sent that does not exist in the world, the environment will exit. + + Args: + name (:obj:`str`): The name of the command, ex "OpenDoor" + num_params (obj:`list` of :obj:`int`): List of arbitrary number parameters + string_params (obj:`list` of :obj:`string`): List of arbitrary string parameters + """ + num_params = [] if num_params is None else num_params + string_params = [] if string_params is None else string_params + + command_to_send = CustomCommand(name, num_params, string_params) + self._enqueue_command(command_to_send) + + def __linux_start_process__( + self, binary_path, task_key, gl_version, verbose, show_viewport=True + ): + import posix_ipc + + out_stream = sys.stdout if verbose else open(os.devnull, "w") + loading_semaphore = posix_ipc.Semaphore( + "/HOLODECK_LOADING_SEM" + self._uuid, + os.O_CREAT | os.O_EXCL, + initial_value=0, + ) + arguments = [ + binary_path, + task_key, + "-HolodeckOn", + "-LOG=HolodeckLog.txt", + "-ForceRes", + "-ResX=" + str(self._window_size[1]), + "-ResY=" + str(self._window_size[0]), + "--HolodeckUUID=" + self._uuid, + "-TicksPerSec=" + str(self._ticks_per_sec), + "-FramesPerSec=" + str(self._frames_per_sec), + "-EnvMinX=" + str(self._env_min[0]), + "-EnvMinY=" + str(self._env_min[1]), + "-EnvMinZ=" + str(self._env_min[2]), + "-EnvMaxX=" + str(self._env_max[0]), + "-EnvMaxY=" + str(self._env_max[1]), + "-EnvMaxZ=" + str(self._env_max[2]), + "-OctreeMin=" + str(self._octree_min), + "-OctreeMax=" + str(self._octree_max), + ] + + if not show_viewport: + arguments.append("-RenderOffScreen") + + self._world_process = subprocess.Popen( + arguments, stdout=out_stream, stderr=out_stream + ) + + atexit.register(self.__on_exit__) + + try: + loading_semaphore.acquire(30) + except posix_ipc.BusyError: + raise HoloOceanException( + "Timed out waiting for binary to load. Ensure that holoocean " + "is not being run with root priveleges." + ) + loading_semaphore.unlink() + + def __windows_start_process__( + self, binary_path, task_key, verbose, show_viewport=True + ): + import win32event + + out_stream = sys.stdout if verbose else open(os.devnull, "w") + loading_semaphore = win32event.CreateSemaphore( + None, 0, 1, "Global\\HOLODECK_LOADING_SEM" + self._uuid + ) + + arguments = [ + binary_path, + task_key, + "-HolodeckOn", + "-LOG=HolodeckLog.txt", + "-ForceRes", + "-ResX=" + str(self._window_size[1]), + "-ResY=" + str(self._window_size[0]), + "--HolodeckUUID=" + self._uuid, + "-TicksPerSec=" + str(self._ticks_per_sec), + "-FramesPerSec=" + str(self._frames_per_sec), + "-EnvMinX=" + str(self._env_min[0]), + "-EnvMinY=" + str(self._env_min[1]), + "-EnvMinZ=" + str(self._env_min[2]), + "-EnvMaxX=" + str(self._env_max[0]), + "-EnvMaxY=" + str(self._env_max[1]), + "-EnvMaxZ=" + str(self._env_max[2]), + "-OctreeMin=" + str(self._octree_min), + "-OctreeMax=" + str(self._octree_max), + ] + + if not show_viewport: + arguments.append("-RenderOffScreen") + + self._world_process = subprocess.Popen( + arguments, stdout=out_stream, stderr=out_stream + ) + + atexit.register(self.__on_exit__) + response = win32event.WaitForSingleObject( + loading_semaphore, 100000 + ) # 100 second timeout + if response == win32event.WAIT_TIMEOUT: + raise HoloOceanException("Timed out waiting for binary to load") + + def __on_exit__(self): + if hasattr(self, "_exited"): + return + + if hasattr(self,"_client"): + self._client.unlink() + if hasattr(self, "_world_process"): + self._world_process.kill() + self._world_process.wait(10) + + self._exited = True + + # Context manager APIs, allows `with` statement to be used + def __enter__(self): + return self + + def __exit__(self, exc_type, exc_value, traceback): + # TODO: Surpress exceptions? + self.__on_exit__() + + def _tick_sensor(self): + for agent_name, agent in self.agents.items(): + for sensor_name, sensor in agent.sensors.items(): + # if it's not time, tick the sensor + if sensor.tick_count != sensor.tick_every: + sensor.tick_count += 1 + # otherwise remove, it and reset count + else: + sensor.tick_count = 1 + + def _get_single_state(self): + if self._agent is not None: + # rebuild state dictionary to drop/change data as needed + if self._copy_state: + state = dict() + for sensor_name, sensor in self.agents[ + self._agent.name + ].sensors.items(): + data = sensor.sensor_data + if isinstance(data, np.ndarray): + state[sensor_name] = np.copy(data) + elif data is not None: + state[sensor_name] = data + + state["t"] = self._num_ticks / self._ticks_per_sec + + else: + state = self._state_dict[self._agent.name] + + return state + + return self._get_full_state() + + def _get_full_state(self): + # rebuild state dictionary to drop/change data as needed + if self._copy_state: + state = dict() + for agent_name, agent in self.agents.items(): + state[agent_name] = dict() + for sensor_name, sensor in agent.sensors.items(): + data = sensor.sensor_data + if isinstance(data, np.ndarray): + state[agent_name][sensor_name] = np.copy(data) + elif data is not None: + state[agent_name][sensor_name] = data + + state["t"] = self._num_ticks / self._ticks_per_sec + + else: + state = self._state_dict + + return state + + def get_reward_terminal(self): + """Returns the reward and terminal state + + Returns: + (:obj:`float`, :obj:`bool`): + A 2tuple: + + - Reward (:obj:`float`): Reward returned by the environment. + - Terminal: The bool terminal signal returned by the environment. + + """ + reward = None + terminal = None + if self._agent is not None: + for sensor in self._state_dict[self._agent.name]: + if "Task" in sensor: + reward = self._state_dict[self._agent.name][sensor][0] + terminal = self._state_dict[self._agent.name][sensor][1] == 1 + return reward, terminal + + def _create_copy(self, obj): + if isinstance(obj, dict): # Deep copy dictionary + copy = dict() + for k, v in obj.items(): + if isinstance(v, dict): + copy[k] = self._create_copy(v) + else: + copy[k] = np.copy(v) + return copy + return None # Not implemented for other types + + ######################### HOLOOCEAN CUSTOM ############################# + + ######################## ACOUSTIC BEACON HELPERS ########################### + + def send_acoustic_message(self, id_from, id_to, msg_type, msg_data): + """Send a message from one beacon to another. + + Args: + id_from (:obj:`int`): The integer ID of the transmitting modem. + id_to (:obj:`int`): The integer ID of the receiving modem. + msg_type (:obj:`str`): The message type. See :class:`holoocean.sensors.AcousticBeaconSensor` for a list. + msg_data : The message to be transmitted. Currently can be any python object. + """ + AcousticBeaconSensor.instances[id_from].send_message(id_to, msg_type, msg_data) + + @property + def beacons(self): + """Gets all instances of AcousticBeaconSensor in the environment. + + Returns: + (:obj:`list` of :obj:`AcousticBeaconSensor`): List of all AcousticBeaconSensor in environment + """ + return AcousticBeaconSensor.instances + + @property + def beacons_id(self): + """Gets all ids of AcousticBeaconSensor in the environment. + + Returns: + (:obj:`list` of :obj:`int`): List of all AcousticBeaconSensor ids in environment + """ + return list(AcousticBeaconSensor.instances.keys()) + + @property + def beacons_status(self): + """Gets all status of AcousticBeaconSensor in the environment. + + Returns: + (:obj:`list` of :obj:`str`): List of all AcousticBeaconSensor status in environment + """ + return [i.status for i in AcousticBeaconSensor.instances.values()] + + ####################### OPTICAL MODEM HELPERS ############################### + + def send_optical_message(self, id_from, id_to, msg_data): + """Sends data between various instances of OpticalModemSensor + + Args: + id_from (:obj:`int`): The integer ID of the transmitting modem. + id_to (:obj:`int`): The integer ID of the receiving modem. + msg_data : The message to be transmitted. Currently can be any python object. + """ + + OpticalModemSensor.instances[id_from].send_message(id_to, msg_data) + + @property + def modems(self): + """Gets all instances of OpticalModemSensor in the environment. + + Returns: + (:obj:`list` of :obj:`OpticalModemSensor`): List of all OpticalModemSensor in environment + """ + return OpticalModemSensor.instances + + @property + def modems_id(self): + """Gets all ids of OpticalModemSensor in the environment. + + Returns: + (:obj:`list` of :obj:`int`): List of all OpticalModemSensor ids in environment + """ + return list(OpticalModemSensor.instances.keys()) diff --git a/PythonAPI/carla/source/holoocean/exceptions.py b/PythonAPI/carla/source/holoocean/exceptions.py new file mode 100644 index 0000000000..bb74dcd43d --- /dev/null +++ b/PythonAPI/carla/source/holoocean/exceptions.py @@ -0,0 +1,22 @@ +"""HoloOcean Exceptions""" + + +class HoloOceanException(Exception): + """Base class for a generic exception in HoloOcean. + + Args: + message (str): The error string. + """ + + +class HoloOceanConfigurationException(HoloOceanException): + """The user provided an invalid configuration for HoloOcean""" + + +class TimeoutException(HoloOceanException): + """Exception raised when communicating with the engine timed out.""" + + +class NotFoundException(HoloOceanException): + """Raised when a package cannot be found""" + diff --git a/PythonAPI/carla/source/holoocean/holoocean.py b/PythonAPI/carla/source/holoocean/holoocean.py new file mode 100644 index 0000000000..1e9cea8841 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/holoocean.py @@ -0,0 +1,103 @@ +"""Module containing high level interface for loading environments.""" +import uuid + +from holoocean.environments import HoloOceanEnvironment +from holoocean.packagemanager import get_scenario,\ + get_binary_path_for_scenario,\ + get_package_config_for_scenario,\ + get_binary_path_for_package +from holoocean.exceptions import HoloOceanException + + +class GL_VERSION: + """OpenGL Version enum. + + Attributes: + OPENGL3 (:obj:`int`): The value for OpenGL3. + OPENGL4 (:obj:`int`): The value for OpenGL4. + """ + OPENGL4 = 4 + OPENGL3 = 3 + + +def make(scenario_name="", scenario_cfg=None, gl_version=GL_VERSION.OPENGL4, window_res=None, verbose=False, + show_viewport=True, ticks_per_sec=None, frames_per_sec=None, copy_state=True): + """Creates a HoloOcean environment + + Args: + world_name (:obj:`str`): + The name of the world to load as an environment. Must match the name of a world in an + installed package. + + scenario_cfg (:obj:`dict`): Dictionary containing scenario configuration, instead of loading a scenario + from the installed packages. Dictionary should match the format of the JSON configuration files + + gl_version (:obj:`int`, optional): + The OpenGL version to use (Linux only). Defaults to GL_VERSION.OPENGL4. + + window_res ((:obj:`int`, :obj:`int`), optional): + The (height, width) to load the engine window at. Overrides the (optional) resolution in the + scenario config file + + verbose (:obj:`bool`, optional): + Whether to run in verbose mode. Defaults to False. + + show_viewport (:obj:`bool`, optional): + If the viewport window should be shown on-screen (Linux only). Defaults to True + + ticks_per_sec (:obj:`int`, optional): + The number of frame ticks per unreal seconds. This will override whatever is + in the configuration json. Defaults to 30. + + frames_per_sec (:obj:`int` or :obj:`bool`, optional): + The max number of frames ticks per real seconds. This will override whatever is + in the configuration json. If True, will match ticks_per_sec. If False, will not be + turned on. If an integer, will set to that value. Defaults to True. + + copy_state (:obj:`bool`, optional): + If the state should be copied or passed as a reference when returned. Defaults to True + + Returns: + :class:`~holoocean.environments.HoloOceanEnvironment`: A holoocean environment instantiated + with all the settings necessary for the specified world, and other supplied arguments. + + """ + + param_dict = dict() + binary_path = None + + if scenario_name != "": + scenario = get_scenario(scenario_name) + binary_path = get_binary_path_for_scenario(scenario_name) + elif scenario_cfg is not None: + scenario = scenario_cfg + binary_path = get_binary_path_for_package(scenario["package_name"]) + else: + raise HoloOceanException("You must specify scenario_name or scenario_config") + + # Get pre-start steps + package_config = get_package_config_for_scenario(scenario) + world = [world for world in package_config["worlds"] if world["name"] == scenario["world"]][0] + param_dict["pre_start_steps"] = world["pre_start_steps"] + + if "env_min" not in scenario and "env_min" in world: + scenario["env_min"] = world["env_min"] + scenario["env_max"] = world["env_max"] + + param_dict["ticks_per_sec"] = ticks_per_sec + param_dict["frames_per_sec"] = frames_per_sec + + param_dict["scenario"] = scenario + param_dict["binary_path"] = binary_path + + param_dict["start_world"] = True + param_dict["uuid"] = str(uuid.uuid4()) + param_dict["gl_version"] = gl_version + param_dict["verbose"] = verbose + param_dict["show_viewport"] = show_viewport + param_dict["copy_state"] = copy_state + + if window_res is not None: + param_dict["window_size"] = window_res + + return HoloOceanEnvironment(**param_dict) diff --git a/PythonAPI/carla/source/holoocean/holooceanclient.py b/PythonAPI/carla/source/holoocean/holooceanclient.py new file mode 100644 index 0000000000..c283dff724 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/holooceanclient.py @@ -0,0 +1,126 @@ +"""The client used for subscribing shared memory between python and c++.""" +import os + +from holoocean.exceptions import HoloOceanException +from holoocean.shmem import Shmem + + +class HoloOceanClient: + """HoloOceanClient for controlling a shared memory session. + + Args: + uuid (:obj:`str`, optional): A UUID to indicate which server this client is associated with. + The same UUID should be passed to the world through a command line flag. Defaults to "". + """ + + def __init__(self, uuid=""): + self._uuid = uuid + + # Important functions + self._get_semaphore_fn = None + self._release_semaphore_fn = None + self._semaphore1 = None + self._semaphore2 = None + self.unlink = None + self.command_center = None + + self._memory = dict() + self._sensors = dict() + self._agents = dict() + self._settings = dict() + + if os.name == "nt": + self.__windows_init__() + elif os.name == "posix": + self.__posix_init__() + else: + raise HoloOceanException("Currently unsupported os: " + os.name) + + def __windows_init__(self): + import win32event + + semaphore_all_access = 0x1F0003 + + self._semaphore1 = win32event.OpenSemaphore( + semaphore_all_access, + False, + "Global\\HOLODECK_SEMAPHORE_SERVER" + self._uuid, + ) + self._semaphore2 = win32event.OpenSemaphore( + semaphore_all_access, + False, + "Global\\HOLODECK_SEMAPHORE_CLIENT" + self._uuid, + ) + + def windows_acquire_semaphore(sem, timeout): + result = win32event.WaitForSingleObject(sem, timeout * 1000) + + if result != win32event.WAIT_OBJECT_0: + raise TimeoutError("Timed out or error waiting for engine!") + + def windows_release_semaphore(sem): + win32event.ReleaseSemaphore(sem, 1) + + def windows_unlink(): + pass + + self._get_semaphore_fn = windows_acquire_semaphore + self._release_semaphore_fn = windows_release_semaphore + self.unlink = windows_unlink + + def __posix_init__(self): + import posix_ipc + + self._semaphore1 = posix_ipc.Semaphore( + "/HOLODECK_SEMAPHORE_SERVER" + self._uuid + ) + self._semaphore2 = posix_ipc.Semaphore( + "/HOLODECK_SEMAPHORE_CLIENT" + self._uuid + ) + + # Unfortunately, OSX doesn't support sem_timedwait(), so setting this timeout + # does nothing. + def posix_acquire_semaphore(sem, timeout): + sem.acquire(timeout) + + def posix_release_semaphore(sem): + sem.release() + + def posix_unlink(): + posix_ipc.unlink_semaphore(self._semaphore1.name) + posix_ipc.unlink_semaphore(self._semaphore2.name) + for shmem_block in self._memory.values(): + shmem_block.unlink() + + self._get_semaphore_fn = posix_acquire_semaphore + self._release_semaphore_fn = posix_release_semaphore + self.unlink = posix_unlink + + def acquire(self, timeout=60): + """Used to acquire control. Will wait until the HolodeckServer has finished its work.""" + self._get_semaphore_fn(self._semaphore2, timeout) + + def release(self): + """Used to release control. Will allow the HolodeckServer to take a step.""" + self._release_semaphore_fn(self._semaphore1) + + def malloc(self, key, shape, dtype): + """Allocates a block of shared memory, and returns a numpy array whose data corresponds + with that block. + + Args: + key (:obj:`str`): The key to identify the block. + shape (:obj:`list` of :obj:`int`): The shape of the numpy array to allocate. + dtype (type): The numpy data type (e.g. np.float32). + + Returns: + :obj:`np.ndarray`: The numpy array that is positioned on the shared memory. + """ + if ( + key not in self._memory + or self._memory[key].shape != shape + or self._memory[key].dtype != dtype + ): + self._memory[key] = Shmem(key, shape, dtype, self._uuid) + + return self._memory[key].np_array diff --git a/PythonAPI/carla/source/holoocean/joint_constraints.py b/PythonAPI/carla/source/holoocean/joint_constraints.py new file mode 100644 index 0000000000..587bba6aeb --- /dev/null +++ b/PythonAPI/carla/source/holoocean/joint_constraints.py @@ -0,0 +1,325 @@ +hand_agent_joints_constraints = { + "hand_r": { + "swing2_limit": 170.0, + "swing1_limit": 170.0, + "twist_limit": 170.0 + }, + "thumb_01_r": { + "swing2_limit": 35.0, + "swing1_limit": 30.0, + "twist_limit": 2.0 + }, + "index_01_r": { + "swing2_limit": 45.0, + "swing1_limit": 10, + "twist_limit": 2.0 + }, + "middle_01_r": { + "swing2_limit": 45.0, + "swing1_limit": 10.0, + "twist_limit": 2.0 + }, + "ring_01_r": { + "swing2_limit": 45.0, + "swing1_limit": 10.0, + "twist_limit": 2.0 + }, + "pinky_01_r": { + "swing2_limit": 45.0, + "swing1_limit": 10, + "twist_limit": 2.0 + }, + "thumb_02_r": { + "swing2_limit": 45.0, + "swing1_limit": 2.0, + "twist_limit": 2.0 + }, + "index_02_r": { + "swing2_limit": 60.0, + "swing1_limit": 0.0, + "twist_limit": 2.0 + }, + "middle_02_r": { + "swing2_limit": 60.0, + "swing1_limit": 0.0, + "twist_limit": 2.0 + }, + "ring_02_r": { + "swing2_limit": 60.0, + "swing1_limit": 0.0, + "twist_limit": 2.0 + }, + "pinky_02_r": { + "swing2_limit": 60.0, + "swing1_limit": 0.0, + "twist_limit": 2.0 + }, + "thumb_03_r": { + "swing2_limit": 45.0, + "swing1_limit": 0.0, + "twist_limit": 0.0 + }, + "index_03_r": { + "swing2_limit": 40.0, + "swing1_limit": 0.0, + "twist_limit": 0.0 + }, + "middle_03_r": { + "swing2_limit": 40.0, + "swing1_limit": 0.0, + "twist_limit": 0.0 + }, + "ring_03_r": { + "swing2_limit": 40.0, + "swing1_limit": 0.0, + "twist_limit": 0.0 + }, + "pinky_03_r": { + "swing2_limit": 40.0, + "swing1_limit": 0.0, + "twist_limit": 0.0 + } +} + +android_agent_joints_constraints = { + "head": { + "swing2_limit": 25, + "swing1_limit": 15, + "twist_limit": 25 + }, + "neck_01": { + "swing2_limit": 0, + "swing1_limit": 25, + "twist_limit": 0 + }, + "spine_02": { + "swing2_limit": 15, + "swing1_limit": 5, + "twist_limit": 25 + }, + "spine_01": { + "swing2_limit": 10, + "swing1_limit": 35, + "twist_limit": 20 + }, + "upperarm_l": { + "swing2_limit": 68.0275, + "swing1_limit": 90, + "twist_limit": 50 + }, + "lowerarm_l": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 0 + }, + "hand_l": { + "swing2_limit": 35, + "swing1_limit": 70, + "twist_limit": 70 + }, + "upperarm_r": { + "swing2_limit": 68.0275, + "swing1_limit": 90, + "twist_limit": 50 + }, + "lowerarm_r": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 0 + }, + "hand_r": { + "swing2_limit": 35, + "swing1_limit": 70, + "twist_limit": 70 + }, + "thigh_l": { + "swing2_limit": 20, + "swing1_limit": 40, + "twist_limit": 60 + }, + "calf_l": { + "swing2_limit": 1, + "swing1_limit": 65, + "twist_limit": 1 + }, + "foot_l": { + "swing2_limit": 25, + "swing1_limit": 40, + "twist_limit": 2 + }, + "ball_l": { + "swing2_limit": 10, + "swing1_limit": 10, + "twist_limit": 15 + }, + "thigh_r": { + "swing2_limit": 20, + "swing1_limit": 40, + "twist_limit": 60 + }, + "calf_r": { + "swing2_limit": 1, + "swing1_limit": 65, + "twist_limit": 1 + }, + "foot_r": { + "swing2_limit": 25, + "swing1_limit": 40, + "twist_limit": 2 + }, + "ball_r": { + "swing2_limit": 10, + "swing1_limit": 10, + "twist_limit": 15 + }, + "thumb_01_l": { + "swing2_limit": 30, + "swing1_limit": 35, + "twist_limit": 2 + }, + "index_01_l": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "middle_01_l": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "ring_01_l": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "pinky_01_l": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "thumb_01_r": { + "swing2_limit": 30, + "swing1_limit": 35, + "twist_limit": 2 + }, + "index_01_r": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "middle_01_r": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "ring_01_r": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "pinky_01_r": { + "swing2_limit": 10, + "swing1_limit": 45, + "twist_limit": 2 + }, + "thumb_02_l": { + "swing2_limit": 2, + "swing1_limit": 45, + "twist_limit": 2 + }, + "index_02_l": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "middle_02_l": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "ring_02_l": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "pinky_02_l": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "thumb_02_r": { + "swing2_limit": 2, + "swing1_limit": 45, + "twist_limit": 2 + }, + "index_02_r": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "middle_02_r": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "ring_02_r": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "pinky_02_r": { + "swing2_limit": 0, + "swing1_limit": 60, + "twist_limit": 2 + }, + "thumb_03_l": { + "swing2_limit": 0, + "swing1_limit": 45, + "twist_limit": 0 + }, + "index_03_l": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "middle_03_l": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "ring_03_l": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "pinky_03_l": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "thumb_03_r": { + "swing2_limit": 0, + "swing1_limit": 45, + "twist_limit": 0 + }, + "index_03_r": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "middle_03_r": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "ring_03_r": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + }, + "pinky_03_r": { + "swing2_limit": 0, + "swing1_limit": 40, + "twist_limit": 0 + } +} diff --git a/PythonAPI/carla/source/holoocean/lcm/AcousticBeaconSensor.py b/PythonAPI/carla/source/holoocean/lcm/AcousticBeaconSensor.py new file mode 100644 index 0000000000..a8031981a9 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/AcousticBeaconSensor.py @@ -0,0 +1,78 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class AcousticBeaconSensor(object): + __slots__ = ["timestamp", "msg_type", "from_beacon", "azimuth", "elevation", "range", "z"] + + __typenames__ = ["int64_t", "string", "int32_t", "float", "float", "float", "float"] + + __dimensions__ = [None, None, None, None, None, None, None] + + def __init__(self): + self.timestamp = 0 + self.msg_type = "" + self.from_beacon = 0 + self.azimuth = 0.0 + self.elevation = 0.0 + self.range = 0.0 + self.z = 0.0 + + def encode(self): + buf = BytesIO() + buf.write(AcousticBeaconSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + __msg_type_encoded = self.msg_type.encode('utf-8') + buf.write(struct.pack('>I', len(__msg_type_encoded)+1)) + buf.write(__msg_type_encoded) + buf.write(b"\0") + buf.write(struct.pack(">iffff", self.from_beacon, self.azimuth, self.elevation, self.range, self.z)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != AcousticBeaconSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return AcousticBeaconSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = AcousticBeaconSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + __msg_type_len = struct.unpack('>I', buf.read(4))[0] + self.msg_type = buf.read(__msg_type_len)[:-1].decode('utf-8', 'replace') + self.from_beacon, self.azimuth, self.elevation, self.range, self.z = struct.unpack(">iffff", buf.read(20)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if AcousticBeaconSensor in parents: return 0 + tmphash = (0xe7c2b98e4d73047a) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if AcousticBeaconSensor._packed_fingerprint is None: + AcousticBeaconSensor._packed_fingerprint = struct.pack(">Q", AcousticBeaconSensor._get_hash_recursive([])) + return AcousticBeaconSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", AcousticBeaconSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/DVLSensor.py b/PythonAPI/carla/source/holoocean/lcm/DVLSensor.py new file mode 100644 index 0000000000..7102ad2e73 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/DVLSensor.py @@ -0,0 +1,70 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class DVLSensor(object): + __slots__ = ["timestamp", "velocity", "range"] + + __typenames__ = ["int64_t", "float", "float"] + + __dimensions__ = [None, [3], [4]] + + def __init__(self): + self.timestamp = 0 + self.velocity = [ 0.0 for dim0 in range(3) ] + self.range = [ 0.0 for dim0 in range(4) ] + + def encode(self): + buf = BytesIO() + buf.write(DVLSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + buf.write(struct.pack('>3f', *self.velocity[:3])) + buf.write(struct.pack('>4f', *self.range[:4])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != DVLSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return DVLSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = DVLSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.velocity = struct.unpack('>3f', buf.read(12)) + self.range = struct.unpack('>4f', buf.read(16)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if DVLSensor in parents: return 0 + tmphash = (0x1466d45f1efc15b7) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if DVLSensor._packed_fingerprint is None: + DVLSensor._packed_fingerprint = struct.pack(">Q", DVLSensor._get_hash_recursive([])) + return DVLSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", DVLSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/DepthSensor.py b/PythonAPI/carla/source/holoocean/lcm/DepthSensor.py new file mode 100644 index 0000000000..7276721379 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/DepthSensor.py @@ -0,0 +1,65 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class DepthSensor(object): + __slots__ = ["timestamp", "depth"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, None] + + def __init__(self): + self.timestamp = 0 + self.depth = 0.0 + + def encode(self): + buf = BytesIO() + buf.write(DepthSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">qf", self.timestamp, self.depth)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != DepthSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return DepthSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = DepthSensor() + self.timestamp, self.depth = struct.unpack(">qf", buf.read(12)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if DepthSensor in parents: return 0 + tmphash = (0xd97fd0672fc2dc8a) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if DepthSensor._packed_fingerprint is None: + DepthSensor._packed_fingerprint = struct.pack(">Q", DepthSensor._get_hash_recursive([])) + return DepthSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", DepthSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/GPSSensor.py b/PythonAPI/carla/source/holoocean/lcm/GPSSensor.py new file mode 100644 index 0000000000..da0688fd8b --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/GPSSensor.py @@ -0,0 +1,67 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class GPSSensor(object): + __slots__ = ["timestamp", "position"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, [3]] + + def __init__(self): + self.timestamp = 0 + self.position = [ 0.0 for dim0 in range(3) ] + + def encode(self): + buf = BytesIO() + buf.write(GPSSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + buf.write(struct.pack('>3f', *self.position[:3])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != GPSSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return GPSSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = GPSSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.position = struct.unpack('>3f', buf.read(12)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if GPSSensor in parents: return 0 + tmphash = (0x211cfac3b0dd5486) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if GPSSensor._packed_fingerprint is None: + GPSSensor._packed_fingerprint = struct.pack(">Q", GPSSensor._get_hash_recursive([])) + return GPSSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", GPSSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/IMUSensor.py b/PythonAPI/carla/source/holoocean/lcm/IMUSensor.py new file mode 100644 index 0000000000..6e61f5e758 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/IMUSensor.py @@ -0,0 +1,76 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class IMUSensor(object): + __slots__ = ["timestamp", "acceleration", "angular_velocity", "acceleration_bias", "angular_velocity_bias"] + + __typenames__ = ["int64_t", "float", "float", "float", "float"] + + __dimensions__ = [None, [3], [3], [3], [3]] + + def __init__(self): + self.timestamp = 0 + self.acceleration = [ 0.0 for dim0 in range(3) ] + self.angular_velocity = [ 0.0 for dim0 in range(3) ] + self.acceleration_bias = [ 0.0 for dim0 in range(3) ] + self.angular_velocity_bias = [ 0.0 for dim0 in range(3) ] + + def encode(self): + buf = BytesIO() + buf.write(IMUSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + buf.write(struct.pack('>3f', *self.acceleration[:3])) + buf.write(struct.pack('>3f', *self.angular_velocity[:3])) + buf.write(struct.pack('>3f', *self.acceleration_bias[:3])) + buf.write(struct.pack('>3f', *self.angular_velocity_bias[:3])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != IMUSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return IMUSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = IMUSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.acceleration = struct.unpack('>3f', buf.read(12)) + self.angular_velocity = struct.unpack('>3f', buf.read(12)) + self.acceleration_bias = struct.unpack('>3f', buf.read(12)) + self.angular_velocity_bias = struct.unpack('>3f', buf.read(12)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if IMUSensor in parents: return 0 + tmphash = (0x400bb3336650f25a) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if IMUSensor._packed_fingerprint is None: + IMUSensor._packed_fingerprint = struct.pack(">Q", IMUSensor._get_hash_recursive([])) + return IMUSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", IMUSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/ImagingSonar.py b/PythonAPI/carla/source/holoocean/lcm/ImagingSonar.py new file mode 100644 index 0000000000..265be8c36d --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/ImagingSonar.py @@ -0,0 +1,72 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class ImagingSonar(object): + __slots__ = ["timestamp", "bins_azimuth", "bins_range", "image"] + + __typenames__ = ["int64_t", "int32_t", "int32_t", "float"] + + __dimensions__ = [None, None, None, ["bins_range", "bins_azimuth"]] + + def __init__(self): + self.timestamp = 0 + self.bins_azimuth = 0 + self.bins_range = 0 + self.image = [] + + def encode(self): + buf = BytesIO() + buf.write(ImagingSonar._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">qii", self.timestamp, self.bins_azimuth, self.bins_range)) + for i0 in range(self.bins_range): + buf.write(struct.pack('>%df' % self.bins_azimuth, *self.image[i0][:self.bins_azimuth])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != ImagingSonar._get_packed_fingerprint(): + raise ValueError("Decode error") + return ImagingSonar._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = ImagingSonar() + self.timestamp, self.bins_azimuth, self.bins_range = struct.unpack(">qii", buf.read(16)) + self.image = [] + for i0 in range(self.bins_range): + self.image.append(struct.unpack('>%df' % self.bins_azimuth, buf.read(self.bins_azimuth * 4))) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if ImagingSonar in parents: return 0 + tmphash = (0x3c5e617d2bbe6ae8) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if ImagingSonar._packed_fingerprint is None: + ImagingSonar._packed_fingerprint = struct.pack(">Q", ImagingSonar._get_hash_recursive([])) + return ImagingSonar._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", ImagingSonar._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/LocationSensor.py b/PythonAPI/carla/source/holoocean/lcm/LocationSensor.py new file mode 100644 index 0000000000..15c31525db --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/LocationSensor.py @@ -0,0 +1,67 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class LocationSensor(object): + __slots__ = ["timestamp", "position"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, [3]] + + def __init__(self): + self.timestamp = 0 + self.position = [ 0.0 for dim0 in range(3) ] + + def encode(self): + buf = BytesIO() + buf.write(LocationSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + buf.write(struct.pack('>3f', *self.position[:3])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != LocationSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return LocationSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = LocationSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.position = struct.unpack('>3f', buf.read(12)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if LocationSensor in parents: return 0 + tmphash = (0x211cfac3b0dd5486) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if LocationSensor._packed_fingerprint is None: + LocationSensor._packed_fingerprint = struct.pack(">Q", LocationSensor._get_hash_recursive([])) + return LocationSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", LocationSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/OrientationSensor.py b/PythonAPI/carla/source/holoocean/lcm/OrientationSensor.py new file mode 100644 index 0000000000..e05199b91d --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/OrientationSensor.py @@ -0,0 +1,70 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class OrientationSensor(object): + __slots__ = ["timestamp", "matrix"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, [3, 3]] + + def __init__(self): + self.timestamp = 0 + self.matrix = [ [ 0.0 for dim1 in range(3) ] for dim0 in range(3) ] + + def encode(self): + buf = BytesIO() + buf.write(OrientationSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + for i0 in range(3): + buf.write(struct.pack('>3f', *self.matrix[i0][:3])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != OrientationSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return OrientationSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = OrientationSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.matrix = [] + for i0 in range(3): + self.matrix.append(struct.unpack('>3f', buf.read(12))) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if OrientationSensor in parents: return 0 + tmphash = (0x5b2182006827a63) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if OrientationSensor._packed_fingerprint is None: + OrientationSensor._packed_fingerprint = struct.pack(">Q", OrientationSensor._get_hash_recursive([])) + return OrientationSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", OrientationSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/PoseSensor.py b/PythonAPI/carla/source/holoocean/lcm/PoseSensor.py new file mode 100644 index 0000000000..0ea3d0b82f --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/PoseSensor.py @@ -0,0 +1,70 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class PoseSensor(object): + __slots__ = ["timestamp", "matrix"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, [4, 4]] + + def __init__(self): + self.timestamp = 0 + self.matrix = [ [ 0.0 for dim1 in range(4) ] for dim0 in range(4) ] + + def encode(self): + buf = BytesIO() + buf.write(PoseSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + for i0 in range(4): + buf.write(struct.pack('>4f', *self.matrix[i0][:4])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != PoseSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return PoseSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = PoseSensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.matrix = [] + for i0 in range(4): + self.matrix.append(struct.unpack('>4f', buf.read(16))) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if PoseSensor in parents: return 0 + tmphash = (0x5b2182005827a64) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if PoseSensor._packed_fingerprint is None: + PoseSensor._packed_fingerprint = struct.pack(">Q", PoseSensor._get_hash_recursive([])) + return PoseSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", PoseSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/RGBCamera.py b/PythonAPI/carla/source/holoocean/lcm/RGBCamera.py new file mode 100644 index 0000000000..f775655c12 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/RGBCamera.py @@ -0,0 +1,76 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class RGBCamera(object): + __slots__ = ["timestamp", "width", "height", "channels", "image"] + + __typenames__ = ["int64_t", "int32_t", "int32_t", "int32_t", "int16_t"] + + __dimensions__ = [None, None, None, None, ["height", "width", "channels"]] + + def __init__(self): + self.timestamp = 0 + self.width = 0 + self.height = 0 + self.channels = 0 + self.image = [] + + def encode(self): + buf = BytesIO() + buf.write(RGBCamera._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">qiii", self.timestamp, self.width, self.height, self.channels)) + for i0 in range(self.height): + for i1 in range(self.width): + buf.write(struct.pack('>%dh' % self.channels, *self.image[i0][i1][:self.channels])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != RGBCamera._get_packed_fingerprint(): + raise ValueError("Decode error") + return RGBCamera._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = RGBCamera() + self.timestamp, self.width, self.height, self.channels = struct.unpack(">qiii", buf.read(20)) + self.image = [] + for i0 in range(self.height): + self.image.append([]) + for i1 in range(self.width): + self.image[i0].append(struct.unpack('>%dh' % self.channels, buf.read(self.channels * 2))) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if RGBCamera in parents: return 0 + tmphash = (0x12773a907ecce8a3) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if RGBCamera._packed_fingerprint is None: + RGBCamera._packed_fingerprint = struct.pack(">Q", RGBCamera._get_hash_recursive([])) + return RGBCamera._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", RGBCamera._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/RangeFinderSensor.py b/PythonAPI/carla/source/holoocean/lcm/RangeFinderSensor.py new file mode 100644 index 0000000000..b4b7440b5e --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/RangeFinderSensor.py @@ -0,0 +1,71 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class RangeFinderSensor(object): + __slots__ = ["timestamp", "count", "distances", "angles"] + + __typenames__ = ["int64_t", "int32_t", "float", "float"] + + __dimensions__ = [None, None, ["count"], ["count"]] + + def __init__(self): + self.timestamp = 0 + self.count = 0 + self.distances = [] + self.angles = [] + + def encode(self): + buf = BytesIO() + buf.write(RangeFinderSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">qi", self.timestamp, self.count)) + buf.write(struct.pack('>%df' % self.count, *self.distances[:self.count])) + buf.write(struct.pack('>%df' % self.count, *self.angles[:self.count])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != RangeFinderSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return RangeFinderSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = RangeFinderSensor() + self.timestamp, self.count = struct.unpack(">qi", buf.read(12)) + self.distances = struct.unpack('>%df' % self.count, buf.read(self.count * 4)) + self.angles = struct.unpack('>%df' % self.count, buf.read(self.count * 4)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if RangeFinderSensor in parents: return 0 + tmphash = (0x6fe5dbaa3d529ab8) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if RangeFinderSensor._packed_fingerprint is None: + RangeFinderSensor._packed_fingerprint = struct.pack(">Q", RangeFinderSensor._get_hash_recursive([])) + return RangeFinderSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", RangeFinderSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/RotationSensor.py b/PythonAPI/carla/source/holoocean/lcm/RotationSensor.py new file mode 100644 index 0000000000..8f29fc2933 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/RotationSensor.py @@ -0,0 +1,67 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class RotationSensor(object): + __slots__ = ["timestamp", "roll", "pitch", "yaw"] + + __typenames__ = ["int64_t", "float", "float", "float"] + + __dimensions__ = [None, None, None, None] + + def __init__(self): + self.timestamp = 0 + self.roll = 0.0 + self.pitch = 0.0 + self.yaw = 0.0 + + def encode(self): + buf = BytesIO() + buf.write(RotationSensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">qfff", self.timestamp, self.roll, self.pitch, self.yaw)) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != RotationSensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return RotationSensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = RotationSensor() + self.timestamp, self.roll, self.pitch, self.yaw = struct.unpack(">qfff", buf.read(20)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if RotationSensor in parents: return 0 + tmphash = (0xa57d6ed5ba175b17) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if RotationSensor._packed_fingerprint is None: + RotationSensor._packed_fingerprint = struct.pack(">Q", RotationSensor._get_hash_recursive([])) + return RotationSensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", RotationSensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/VelocitySensor.py b/PythonAPI/carla/source/holoocean/lcm/VelocitySensor.py new file mode 100644 index 0000000000..736a33b390 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/VelocitySensor.py @@ -0,0 +1,67 @@ +"""LCM type definitions +This file automatically generated by lcm. +DO NOT MODIFY BY HAND!!!! +""" + +try: + import cStringIO.StringIO as BytesIO +except ImportError: + from io import BytesIO +import struct + +class VelocitySensor(object): + __slots__ = ["timestamp", "velocity"] + + __typenames__ = ["int64_t", "float"] + + __dimensions__ = [None, [3]] + + def __init__(self): + self.timestamp = 0 + self.velocity = [ 0.0 for dim0 in range(3) ] + + def encode(self): + buf = BytesIO() + buf.write(VelocitySensor._get_packed_fingerprint()) + self._encode_one(buf) + return buf.getvalue() + + def _encode_one(self, buf): + buf.write(struct.pack(">q", self.timestamp)) + buf.write(struct.pack('>3f', *self.velocity[:3])) + + def decode(data): + if hasattr(data, 'read'): + buf = data + else: + buf = BytesIO(data) + if buf.read(8) != VelocitySensor._get_packed_fingerprint(): + raise ValueError("Decode error") + return VelocitySensor._decode_one(buf) + decode = staticmethod(decode) + + def _decode_one(buf): + self = VelocitySensor() + self.timestamp = struct.unpack(">q", buf.read(8))[0] + self.velocity = struct.unpack('>3f', buf.read(12)) + return self + _decode_one = staticmethod(_decode_one) + + def _get_hash_recursive(parents): + if VelocitySensor in parents: return 0 + tmphash = (0x13111cc3baf33cae) & 0xffffffffffffffff + tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff + return tmphash + _get_hash_recursive = staticmethod(_get_hash_recursive) + _packed_fingerprint = None + + def _get_packed_fingerprint(): + if VelocitySensor._packed_fingerprint is None: + VelocitySensor._packed_fingerprint = struct.pack(">Q", VelocitySensor._get_hash_recursive([])) + return VelocitySensor._packed_fingerprint + _get_packed_fingerprint = staticmethod(_get_packed_fingerprint) + + def get_hash(self): + """Get the LCM hash of the struct""" + return struct.unpack(">Q", VelocitySensor._get_packed_fingerprint())[0] + diff --git a/PythonAPI/carla/source/holoocean/lcm/__init__.py b/PythonAPI/carla/source/holoocean/lcm/__init__.py new file mode 100644 index 0000000000..29f59dce07 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/__init__.py @@ -0,0 +1,19 @@ +"""LCM package __init__.py file +This file automatically generated by lcm-gen. +DO NOT MODIFY BY HAND!!!! +""" + +from .DVLSensor import DVLSensor +from .IMUSensor import IMUSensor +from .GPSSensor import GPSSensor +from .AcousticBeaconSensor import AcousticBeaconSensor +from .ImagingSonar import ImagingSonar +from .DepthSensor import DepthSensor +from .RGBCamera import RGBCamera +from .PoseSensor import PoseSensor +from .LocationSensor import LocationSensor +from .RangeFinderSensor import RangeFinderSensor +from .RotationSensor import RotationSensor +from .OrientationSensor import OrientationSensor +from .VelocitySensor import VelocitySensor +from .main import SensorData, gen \ No newline at end of file diff --git a/PythonAPI/carla/source/holoocean/lcm/main.py b/PythonAPI/carla/source/holoocean/lcm/main.py new file mode 100644 index 0000000000..ab8061a6b8 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/main.py @@ -0,0 +1,135 @@ +from holoocean.lcm import DVLSensor, IMUSensor, GPSSensor, \ + AcousticBeaconSensor, ImagingSonar, DepthSensor, \ + RGBCamera, PoseSensor, LocationSensor, \ + RangeFinderSensor, RotationSensor, OrientationSensor, \ + VelocitySensor +import numpy as np +import os + +class SensorData: + """Wrapper class for the various types of publishable sensor data. + + Parameters: + sensor_type (:obj:`str`): Type of sensor to be imported + channel (:obj:`str`): Name of channel to publish to. + """ + _sensor_keys_ = { + "DVLSensor": DVLSensor, + "IMUSensor": IMUSensor, + "GPSSensor": GPSSensor, + "AcousticBeaconSensor": AcousticBeaconSensor, + "ImagingSonar": ImagingSonar, + "DepthSensor": DepthSensor, + "RGBCamera": RGBCamera, + "PoseSensor": PoseSensor, + "LocationSensor": LocationSensor, + "RangeFinderSensor": RangeFinderSensor, + "RotationSensor": RotationSensor, + "OrientationSensor": OrientationSensor, + "VelocitySensor": VelocitySensor, + } + + def __init__(self, sensor_type, channel): + self.type = sensor_type + self.msg = self._sensor_keys_[sensor_type]() + self.channel = channel + + def set_value(self, timestamp, value): + """Set value in respective sensor class. + + Parameters: + timestamp (:obj:`int`): Number of milliseconds since last data was published + value (:obj:`list`): List of sensor data to put into LCM sensor class + """ + self.msg.timestamp = timestamp + + if self.type == "DVLSensor": + self.msg.velocity = value[:3].tolist() + if value.shape[0] == 7: + self.msg.range = value[3:].tolist() + else: + self.msg.range = np.full(4, np.NaN) + elif self.type == "IMUSensor": + self.msg.acceleration = value[0].tolist() + self.msg.angular_velocity = value[1].tolist() + if value.shape[0] == 4: + self.msg.acceleration_bias = value[2].tolist() + self.msg.angular_velocity_bias = value[3].tolist() + else: + self.msg.acceleration_bias = np.full(3, np.NaN) + self.msg.angular_velocity_bias = np.full(3, np.NaN) + elif self.type == "GPSSensor": + self.msg.position = value.tolist() + elif self.type == "AcousticBeaconSensor": + self.msg.msg_type = value[0] + self.msg.from_beacon = value[1] + # TODO Eventually somehow handle data passed through in value[2] + self.msg.azimuth = value[3] if value[0] not in ["OWAY", "MSG_REQ", "MSG_RESP"] else np.NaN + self.msg.elevation = value[4] if value[0] not in ["OWAY", "MSG_REQ", "MSG_RESP"] else np.NaN + self.msg.range = value[5] if value[0] in ["MSG_RESPU", "MSG_RESPX"] else np.NaN + self.msg.z = value[-1] if value[0] in ["MSG_REQX", "MSG_RESPX"] else np.NaN + elif self.type == "ImagingSonar": + self.msg.bins_range = value.shape[0] + self.msg.bins_azimuth = value.shape[1] + self.msg.image = value.tolist() + elif self.type == "DepthSensor": + self.msg.depth = value[0] + elif self.type == "RGBCamera": + self.msg.height = value.shape[0] + self.msg.width = value.shape[1] + self.msg.channels = value.shape[2] + self.msg.image = value.tolist() + elif self.type == "PoseSensor": + self.msg.matrix = value.tolist() + elif self.type == "LocationSensor": + self.msg.position = value.tolist() + elif self.type == "RangeFinderSensor": + count = len(value) + self.msg.count = count + self.msg.distances = value.tolist() + self.msg.angles = np.linspace(0, 360, count, endpoint=False).tolist() + elif self.type == "RotationSensor": + self.msg.roll, self.msg.pitch, self.msg.yaw = value + elif self.type == "OrientationSensor": + self.msg.matrix = value.tolist() + elif self.type == "VelocitySensor": + self.msg.velocity = value.tolist() + else: + raise ValueError("That sensor hasn't been implemented in LCM yet") + +def gen(lang, path='.', headers=None): + """Generates LCM files for sensors in whatever language requested. + + Args: + lang (:obj:`str`): One of "cpp", "c", "java", "python", "lua", "csharp", "go" + path (:obj:`str`, optional): Location to save files in. Defaults to current directory. + headers (:obj:`str`, optional): Where to store .h files for C . Defaults to same as c files, given by path arg. + """ + #set up all paths + lcm_path = os.path.join( os.path.dirname(os.path.realpath(__file__)), 'sensors.lcm' ) + path = os.path.abspath(path) + if headers is None: + headers = path + else: + headers = os.path.abspath(headers) + + #determine flags to send with it + if lang == "c": + flags = f"-c --c-cpath {path} --c-hpath {headers}" + elif lang == "cpp": + flags = f"-x --cpp-hpath {path}" + elif lang == "java": + flags = f"-j --jpath {path}" + elif lang == "python": + flags = f"-p --ppath {path}" + elif lang == "lua": + flags = f"-l --lpath {path}" + elif lang == "csharp": + flags = f"--csharp --csharp-path {path}" + elif lang == "go": + flags = f"--go --go-path {path}" + else: + raise ValueError("Not a valid language for LCM files.") + + #run the command + os.system(f"lcm-gen {lcm_path} {flags}") \ No newline at end of file diff --git a/PythonAPI/carla/source/holoocean/lcm/sensors.lcm b/PythonAPI/carla/source/holoocean/lcm/sensors.lcm new file mode 100644 index 0000000000..ed69038849 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/lcm/sensors.lcm @@ -0,0 +1,104 @@ +package lcm; + +struct DVLSensor +{ + int64_t timestamp; + float velocity[3]; + float range[4]; +} + +struct IMUSensor +{ + int64_t timestamp; + float acceleration[3]; + float angular_velocity[3]; + float acceleration_bias[3]; + float angular_velocity_bias[3]; +} + +struct GPSSensor +{ + int64_t timestamp; + float position[3]; +} + +struct AcousticBeaconSensor +{ + int64_t timestamp; + + string msg_type; + int32_t from_beacon; + float azimuth; + float elevation; + float range; + float z; +} + +// TODO: Optical Modem once we solidifed data types for it + +struct ImagingSonar +{ + int64_t timestamp; + + int32_t bins_azimuth; + int32_t bins_range; + + float image[bins_range][bins_azimuth]; +} + +struct DepthSensor +{ + int64_t timestamp; + float depth; +} + +struct RGBCamera +{ + int64_t timestamp; + + int32_t width; + int32_t height; + int32_t channels; + + int16_t image[height][width][channels]; +} + +struct PoseSensor +{ + int64_t timestamp; + float matrix[4][4]; +} + +struct LocationSensor +{ + int64_t timestamp; + float position[3]; +} + +struct RangeFinderSensor +{ + int64_t timestamp; + int32_t count; + float distances[count]; + float angles[count]; +} + +struct RotationSensor +{ + int64_t timestamp; + float roll; + float pitch; + float yaw; +} + +struct OrientationSensor +{ + int64_t timestamp; + float matrix[3][3]; +} + +struct VelocitySensor +{ + int64_t timestamp; + float velocity[3]; +} \ No newline at end of file diff --git a/PythonAPI/carla/source/holoocean/packagemanager.py b/PythonAPI/carla/source/holoocean/packagemanager.py new file mode 100644 index 0000000000..96f20a3d07 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/packagemanager.py @@ -0,0 +1,552 @@ +"""Package manager for worlds available to download and use for HoloOcean""" +import json +import os +import shutil +import sys +import tempfile +import urllib.request +import urllib.error +import fnmatch +import zipfile +import pprint + +import time +import select + +from queue import Queue +from threading import Thread + +from holoocean import util +from holoocean import __version__ +from holoocean.exceptions import HoloOceanException, NotFoundException + + +BACKEND_URL = "https://robots.et.byu.edu/holo/" + +def _get_from_backend(rel_url): + """ + Gets the resource given at rel_url, assumes it is a utf-8 text file + + Args: + rel_url (:obj:`str`): url relative to BACKEND_URL to fetch + + Returns: + :obj:`str`: The resource at rel_url as a string + """ + req = urllib.request.urlopen(BACKEND_URL + rel_url) + data = req.read() + return data.decode('utf-8') + + +def available_packages(): + """Returns a list of package names available for the current version of HoloOcean + + Returns (:obj:`list` of :obj:`str`): + List of package names + """ + # Get the index json file from the backend + url = "{ver}/available".format(ver=util.get_holoocean_version()) + try: + index = _get_from_backend(url) + index = json.loads(index) + except urllib.error.URLError as err: + print("Unable to communicate with backend ({}), {}".format( + url, err.reason), + file=sys.stderr) + raise + + return index["packages"] + + +def installed_packages(): + """Returns a list of all installed packages + + Returns: + :obj:`list` of :obj:`str`: List of all the currently installed packages + """ + _check_for_old_versions() + return [x["name"] for x, _ in _iter_packages()] + + +def package_info(pkg_name): + """Prints the information of a package. + + Args: + pkg_name (:obj:`str`): The name of the desired package to get information + """ + indent = " " + for config, _ in _iter_packages(): + if pkg_name == config["name"]: + print("Package:", pkg_name) + print(indent, "Platform:", config["platform"]) + print(indent, "Version:", config["version"]) + print(indent, "Path:", config["path"]) + print(indent, "Worlds:") + for world in config["worlds"]: + world_info(world["name"], world_config=world, base_indent=4) + + +def _print_agent_info(agents, base_indent=0): + print(base_indent*' ', "Agents:") + base_indent += 2 + for agent in agents: + print(base_indent*' ', "Name:", agent["agent_name"]) + print(base_indent*' ', "Type:", agent["agent_type"]) + print(base_indent*' ', "Sensors:") + for sensor in agent["sensors"]: + print((base_indent + 2)*' ', sensor['sensor_type']) + for k, v in sensor.items(): + if k == "sensor_type": + continue + elif k == "configuration": + print((base_indent + 4)*' ', k) + for opt, val in v.items(): + print((base_indent + 6)*' ', opt+":", val) + else: + print((base_indent + 4)*' ', k+":", v) + + +def world_info(world_name, world_config=None, base_indent=0): + """Gets and prints the information of a world. + + Args: + world_name (:obj:`str`): the name of the world to retrieve information for + world_config (:obj:`dict`, optional): A dictionary containing the world's configuration. + Will find the config if None. Defaults to None. + base_indent (:obj:`int`, optional): How much to indent output + """ + if world_config is None: + for config, _ in _iter_packages(): + for world in config["worlds"]: + if world["name"] == world_name: + world_config = world + + if world_config is None: + raise HoloOceanException("Couldn't find world " + world_name) + + print(base_indent*' ', world_config["name"]) + base_indent += 4 + + if "agents" in world_config: + _print_agent_info(world_config["agents"], base_indent) + + print(base_indent*' ', "Scenarios:") + for scenario, _ in _iter_scenarios(world_name): + scenario_info(scenario=scenario, base_indent=base_indent + 2) + + +def _find_file_in_worlds_dir(filename): + """ + Recursively tries to find filename in the worlds directory of holoocean + + Args: + filename (:obj:`str`): Pattern to try and match (fnmatch) + + Returns: + :obj:`str`: The path or an empty string if the file was not found + + """ + for root, _, filenames in os.walk(util.get_holoocean_path(), "worlds"): + for match in fnmatch.filter(filenames, filename): + return os.path.join(root, match) + return "" + + +def scenario_info(scenario_name="", scenario=None, base_indent=0): + """Gets and prints information for a particular scenario file + Must match this format: scenario_name.json + + Args: + scenario_name (:obj:`str`): The name of the scenario + scenario (:obj:`dict`, optional): Loaded dictionary config + (overrides world_name and scenario_name) + base_indent (:obj:`int`, optional): How much to indent output by + """ + scenario_file = "" + if scenario is None: + # Find this file in the worlds/ directory + filename = '{}.json'.format(scenario_name) + scenario_file = _find_file_in_worlds_dir(filename) + + if scenario_file == "": + raise FileNotFoundError("The file {} could not be found".format(filename)) + + scenario = load_scenario_file(scenario_file) + + print(base_indent*' ', "{}-{}:".format(scenario["world"], scenario["name"])) + base_indent += 2 + if "agents" in scenario: + _print_agent_info(scenario["agents"], base_indent) + +def _validVersion(user_v,min_v): + """Check if user version is less than the minimum version. + Returns true if user_v is equal or above min_v, otherwise returns False. + + Args: + user_v (:obj:`str`): The user's version number + min_v (:obj:`str`): The minimum allowable version number + """ + user = [] + min = [] + + for i in range(0,len(user_v)): + if user_v[i] == '.': + continue + user.append(user_v[i]) + + for i in range(0,len(min_v)): + if min_v[i] == '.': + continue + min.append(min_v[i]) + + for i in range(0,len(min)): + if user[i] < min[i]: + return False + if user[i] > min[i]: + return True + + return True + + +def install(package_name, url=None, branch=None, commit=None): + """Installs a holoocean package. + + Args: + package_name (:obj:`str`): The name of the package to install + """ + + print("The world that you are downloading contains Unreal Engine code and likely content.") + print("Download and/or use of this world signifies agreement that you have already accepted and agree\nto be bound by the Unreal Engine EULA found at https://www.unrealengine.com/en-US/eula/unreal.") + + # input("Press Enter to continue: ") + + user_version =__version__ + min_version = "1.0.0" + if(_validVersion(user_version, min_version) == False): + print("You must update HoloOcean to at least version 1.0.0 to continue. Packages are not available for earlier versions.") + return + + if package_name is None and url is None: + raise HoloOceanException("You must specify the URL or a valid package name") + + if package_name in installed_packages(): + print(f"{package_name} already installed.") + return + + _check_for_old_versions() + holodeck_path = util.get_holoocean_path() + + if url is None: + # If the URL is none, we need to derive it + packages = available_packages() + if package_name not in packages: + print("Package not found. Available packages are:", file=sys.stderr) + pprint.pprint(packages, width=10, indent=4, stream=sys.stderr) + return + + if branch is not None: + if util.get_os_key() != "Linux": + print(f"Can't install from branch when using {util.get_os_key()}") + return + if commit is None: + commit = "latest" + + else: + branch = "v{holodeck_version}".format(holodeck_version=util.get_holoocean_version()) + commit = util.get_os_key() + + # example: %backend%/Ocean/v0.1.0/Linux.zip + url = "{backend_url}{package_name}/{branch}/{platform}.zip".format( + backend_url=BACKEND_URL, + branch=branch, + package_name=package_name, + platform=commit) + + install_path = os.path.join(holodeck_path, "worlds", package_name) + + print("Installing {} from {} to {}".format(package_name, url, install_path)) + + _download_binary(url, install_path) + +def _check_for_old_versions(): + """Checks for old versions of the binary and tells the user they can remove them. + If there is an ignore_old_packages file, it will stay silent. + """ + # holodeckpath turns off the binary folder versioning + if "HOLODECKPATH" in os.environ: + return + + path = util._get_holoocean_folder() + + if not os.path.exists(path): + return + + not_matching = [] + + for f in os.listdir(path): + f_path = os.path.join(path, f) + if f == "ignore_old_packages": + return + if f == util.get_holoocean_version(): + continue + elif not os.path.isfile(f_path): + not_matching.append(f) + + if not_matching: + print("**********************************************") + print("* You have old versions of HoloOcean packages *") + print("**********************************************") + print("Use packagemanager.prune() to delete old packages") + print("Versions:", not_matching) + print("Place an `ignore_old_packages` file in {} to surpress this message".format(path)) + print() + +def prune(): + """Prunes old versions of holoocean, other than the running version. + + **DO NOT USE WITH HOLODECKPATH** + + Don't use this function if you have overidden the path. + """ + if "HOLODECKPATH" in os.environ: + print("This function is not available when using HOLODECKPATH", stream=sys.stderr) + return + + holodeck_folder = util._get_holoocean_folder() + + # Delete everything in holodeck_folder that isn't the current holodeck version + for file in os.listdir(holodeck_folder): + file_path = os.path.join(holodeck_folder, file) + if os.path.isfile(file_path): + continue + if file == util.get_holoocean_version(): + continue + # Delete it! + print("Deleting {}".format(file_path)) + shutil.rmtree(file_path) + + print("Done") + +def remove(package_name): + """Removes a holoocean package. + + Args: + package_name (:obj:`str`): the name of the package to remove + """ + for config, path in _iter_packages(): + if config["name"] == package_name: + shutil.rmtree(path) + + +def remove_all_packages(): + """Removes all holoocean packages. + + """ + for _, path in _iter_packages(): + shutil.rmtree(path) + + +def load_scenario_file(scenario_path): + """ + Loads the scenario config file and returns a dictionary containing the configuration + + Args: + scenario_path (:obj:`str`): Path to the configuration file + + Returns: + :obj:`dict`: A dictionary containing the configuration file + + """ + with open(scenario_path, 'r') as f: + return json.load(f) + + +def get_scenario(scenario_name): + """Gets the scenario configuration associated with the given name + + Args: + scenario_name (:obj:`str`): name of the configuration to load - eg "UrbanCity-Follow" + Must be an exact match. Name must be unique among all installed packages + + Returns: + :obj:`dict`: A dictionary containing the configuration file + + """ + config_path = _find_file_in_worlds_dir(scenario_name + ".json") + + if config_path == "": + raise FileNotFoundError( + "The file `{file}.json` could not be found in {path}. " + "Make sure the package that contains {file} " \ + "is installed.".format(file=scenario_name, path=util.get_holoocean_path())) + + return load_scenario_file(config_path) + + +def get_binary_path_for_package(package_name): + """Gets the path to the binary of a specific package. + + Args: + package_name (:obj:`str`): Name of the package to search for + + Returns: + :obj:`str`: Returns the path to the config directory + + Raises: + NotFoundException: When the package requested is not found + + """ + + for config, path in _iter_packages(): + try: + if config["name"] == package_name: + return os.path.join(path, config["path"]) + except KeyError as e: + print("Error parsing config file for {}".format(path)) + + raise NotFoundException("Package `{}` not found!".format(package_name)) + + +def get_binary_path_for_scenario(scenario_name): + """Gets the path to the binary for a given scenario name + + Args: + scenario_name (:obj:`str`): name of the configuration to load - eg "UrbanCity-Follow" + Must be an exact match. Name must be unique among all installed packages + + Returns: + :obj:`dict`: A dictionary containing the configuration file + + """ + scenario_path = _find_file_in_worlds_dir(scenario_name + ".json") + root = os.path.dirname(scenario_path) + config_path = os.path.join(root, "config.json") + with open(config_path, 'r') as f: + config = json.load(f) + return os.path.join(root, config["path"]) + + +def get_package_config_for_scenario(scenario): + """For the given scenario, returns the package config associated with it (config.json) + + Args: + scenario (:obj:`dict`): scenario dict to look up the package for + + Returns: + :obj:`dict`: package configuration dictionary + + """ + + world_name = scenario["world"] + + for config, path in _iter_packages(): + for world in config["worlds"]: + if world["name"] == world_name: + return config + + raise HoloOceanException("Could not find a package that contains world {}".format(world_name)) + + +def _iter_packages(): + path = util.get_holoocean_path() + worlds_path = os.path.join(path, "worlds") + if not os.path.exists(worlds_path): + os.makedirs(worlds_path) + for dir_name in os.listdir(worlds_path): + full_path = os.path.join(worlds_path, dir_name) + if os.path.isdir(full_path): + for file_name in os.listdir(full_path): + if file_name == "config.json": + with open(os.path.join(full_path, file_name), 'r') as f: + config = json.load(f) + yield config, full_path + + +def _iter_scenarios(world_name): + """Iterates over the scenarios associated with world_name. + + Note that world_name needs to be unique among all packages + + Args: + world_name (:obj:`str`): name of the world + + Returns: config_dict, path_to_config + """ + + # Find a scenario for this world + a_scenario = _find_file_in_worlds_dir("{}-*".format(world_name)) + + if a_scenario is None: + return + + # Find the parent path of that file + world_path = os.path.abspath(os.path.join(a_scenario, os.pardir)) + + if not os.path.exists(world_path): + os.makedirs(world_path) + for file_name in os.listdir(world_path): + if file_name == "config.json": + continue + if not file_name.endswith(".json"): + continue + if not fnmatch.fnmatch(file_name, "{}-*.json".format(world_name)): + continue + + + full_path = os.path.join(world_path, file_name) + with open(full_path, 'r') as f: + config = json.load(f) + yield config, full_path + + +def _download_binary(binary_location, install_location, block_size=1000000): + def file_writer_worker(tmp_fd, length, queue): + max_width = 20 + percent_per_block = 100 // max_width + amount_written = 0 + while amount_written < length: + tmp_fd.write(queue.get()) + amount_written += block_size + percent_done = 100 * amount_written / length + int_percent = int(percent_done) + num_blocks = int_percent // percent_per_block + blocks = chr(0x2588) * num_blocks + spaces = " " * (max_width - num_blocks) + try: + sys.stdout.write("\r|" + blocks + spaces + "| %d%%" % int_percent) + except UnicodeEncodeError: + print("\r" + str(int_percent) + "%", end="") + + sys.stdout.flush() + + queue = Queue() + tmp_fd = tempfile.TemporaryFile(suffix=".zip") + with urllib.request.urlopen(binary_location) as conn: + file_size = int(conn.headers["Content-Length"]) + print("File size:", util.human_readable_size(file_size)) + amount_read = 0 + write_thread = Thread(target=file_writer_worker, args=(tmp_fd, file_size, queue)) + write_thread.start() + while amount_read < file_size: + queue.put(conn.read(block_size)) + amount_read += block_size + write_thread.join() + print() + + # Unzip the binary + # Note the contents of the ZIP file get extracted straight into the install directory, so the + # zip's structure should look like file.zip/config.json not file.zip/file/config.json + print("Unpacking worlds...") + with zipfile.ZipFile(tmp_fd, 'r') as zip_file: + zip_file.extractall(install_location) + + if os.name == "posix": + print("Fixing Permissions") + _make_excecutable(install_location) + + print("Finished.") + +def _make_excecutable(install_path): + for path, _, files in os.walk(install_path): + for f in files: + os.chmod(os.path.join(path, f), 0o777) diff --git a/PythonAPI/carla/source/holoocean/pylintrc b/PythonAPI/carla/source/holoocean/pylintrc new file mode 100644 index 0000000000..d99aafdd49 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/pylintrc @@ -0,0 +1,542 @@ +[MASTER] + +# A comma-separated list of package or module names from where C extensions may +# be loaded. Extensions are loading into the active Python interpreter and may +# run arbitrary code +extension-pkg-whitelist=posix_ipc + +# Add files or directories to the blacklist. They should be base names, not +# paths. +ignore=CVS,docs + +# Add files or directories matching the regex patterns to the blacklist. The +# regex matches against base names, not paths. +ignore-patterns= + +# Python code to execute, usually for sys.path manipulation such as +# pygtk.require(). +#init-hook= + +# Use multiple processes to speed up Pylint. +jobs=4 + +# List of plugins (as comma separated values of python modules names) to load, +# usually to register additional checkers. +load-plugins= + +# Pickle collected data for later comparisons. +persistent=yes + +# Specify a configuration file. +#rcfile= + +# When enabled, pylint would attempt to guess common misconfiguration and emit +# user-friendly hints instead of false-positive error messages +suggestion-mode=yes + +# Allow loading of arbitrary C extensions. Extensions are imported into the +# active Python interpreter and may run arbitrary code. +unsafe-load-any-extension=no + + +[MESSAGES CONTROL] + +# Only show warnings with the listed confidence levels. Leave empty to show +# all. Valid levels: HIGH, INFERENCE, INFERENCE_FAILURE, UNDEFINED +confidence= + +# Disable the message, report, category or checker with the given id(s). You +# can either give multiple identifiers separated by comma (,) or put this +# option multiple times (only on the command line, not in the configuration +# file where it should appear only once).You can also use "--disable=all" to +# disable everything first and then reenable specific checks. For example, if +# you want to run only the similarities checker, you can use "--disable=all +# --enable=similarities". If you want to run only the classes checker, but have +# no Warning level messages displayed, use"--disable=all --enable=classes +# --disable=W" +disable=print-statement, + too-few-public-methods, + parameter-unpacking, + unpacking-in-except, + old-raise-syntax, + backtick, + long-suffix, + old-ne-operator, + old-octal-literal, + import-star-module-level, + non-ascii-bytes-literal, + raw-checker-failed, + bad-inline-option, + locally-disabled, + locally-enabled, + file-ignored, + suppressed-message, + useless-suppression, + deprecated-pragma, + apply-builtin, + basestring-builtin, + buffer-builtin, + cmp-builtin, + coerce-builtin, + execfile-builtin, + file-builtin, + long-builtin, + raw_input-builtin, + reduce-builtin, + standarderror-builtin, + unicode-builtin, + xrange-builtin, + coerce-method, + delslice-method, + getslice-method, + setslice-method, + no-absolute-import, + old-division, + dict-iter-method, + dict-view-method, + next-method-called, + metaclass-assignment, + indexing-exception, + raising-string, + reload-builtin, + oct-method, + hex-method, + nonzero-method, + cmp-method, + input-builtin, + round-builtin, + intern-builtin, + unichr-builtin, + map-builtin-not-iterating, + zip-builtin-not-iterating, + range-builtin-not-iterating, + filter-builtin-not-iterating, + using-cmp-argument, + eq-without-hash, + div-method, + idiv-method, + rdiv-method, + exception-message-attribute, + invalid-str-codec, + sys-max-int, + bad-python3-import, + deprecated-string-function, + deprecated-str-translate-call, + deprecated-itertools-function, + deprecated-types-field, + next-method-defined, + dict-items-not-iterating, + dict-keys-not-iterating, + dict-values-not-iterating + +# Enable the message, report, category or checker with the given id(s). You can +# either give multiple identifier separated by comma (,) or put this option +# multiple time (only on the command line, not in the configuration file where +# it should appear only once). See also the "--disable" option for examples. +enable=c-extension-no-member + + +[REPORTS] + +# Python expression which should return a note less than 10 (10 is the highest +# note). You have access to the variables errors warning, statement which +# respectively contain the number of errors / warnings messages and the total +# number of statements analyzed. This is used by the global evaluation report +# (RP0004). +evaluation=10.0 - ((float(5 * error + warning + refactor + convention) / statement) * 10) + +# Template used to display messages. This is a python new-style format string +# used to format the message information. See doc for all details +#msg-template= + +# Set the output format. Available formats are text, parseable, colorized, json +# and msvs (visual studio).You can also give a reporter class, eg +# mypackage.mymodule.MyReporterClass. +output-format=text + +# Tells whether to display a full report or only the messages +reports=no + +# Activate the evaluation score. +score=yes + + +[REFACTORING] + +# Maximum number of nested blocks for function / method body +max-nested-blocks=5 + +# Complete name of functions that never returns. When checking for +# inconsistent-return-statements if a never returning function is called then +# it will be considered as an explicit return statement and no message will be +# printed. +never-returning-functions=optparse.Values,sys.exit + + +[SIMILARITIES] + +# Ignore comments when computing similarities. +ignore-comments=yes + +# Ignore docstrings when computing similarities. +ignore-docstrings=yes + +# Ignore imports when computing similarities. +ignore-imports=no + +# Minimum lines number of a similarity. +min-similarity-lines=4 + + +[SPELLING] + +# Limits count of emitted suggestions for spelling mistakes +max-spelling-suggestions=4 + +# Spelling dictionary name. Available dictionaries: none. To make it working +# install python-enchant package. +spelling-dict= + +# List of comma separated words that should not be checked. +spelling-ignore-words= + +# A path to a file that contains private dictionary; one word per line. +spelling-private-dict-file= + +# Tells whether to store unknown words to indicated private dictionary in +# --spelling-private-dict-file option instead of raising a message. +spelling-store-unknown-words=no + + +[LOGGING] + +# Logging modules to check that the string format arguments are in logging +# function parameter format +logging-modules=logging + + +[TYPECHECK] + +# List of decorators that produce context managers, such as +# contextlib.contextmanager. Add to this list to register other decorators that +# produce valid context managers. +contextmanager-decorators=contextlib.contextmanager + +# List of members which are set dynamically and missed by pylint inference +# system, and so shouldn't trigger E1101 when accessed. Python regular +# expressions are accepted. +generated-members= + +# Tells whether missing members accessed in mixin class should be ignored. A +# mixin class is detected if its name ends with "mixin" (case insensitive). +ignore-mixin-members=yes + +# This flag controls whether pylint should warn about no-member and similar +# checks whenever an opaque object is returned when inferring. The inference +# can return multiple potential results while evaluating a Python object, but +# some branches might not be evaluated, which results in partial inference. In +# that case, it might be useful to still emit no-member and other checks for +# the rest of the inferred objects. +ignore-on-opaque-inference=yes + +# List of class names for which member attributes should not be checked (useful +# for classes with dynamically set attributes). This supports the use of +# qualified names. +ignored-classes=optparse.Values,thread._local,_thread._local + +# List of module names for which member attributes should not be checked +# (useful for modules/projects where namespaces are manipulated during runtime +# and thus existing member attributes cannot be deduced by static analysis. It +# supports qualified module names, as well as Unix pattern matching. +ignored-modules= + +# Show a hint with possible names when a member name was not found. The aspect +# of finding the hint is based on edit distance. +missing-member-hint=yes + +# The minimum edit distance a name should have in order to be considered a +# similar match for a missing member name. +missing-member-hint-distance=1 + +# The total number of similar names that should be taken in consideration when +# showing a hint for a missing member. +missing-member-max-choices=1 + + +[BASIC] + +# Naming style matching correct argument names +argument-naming-style=snake_case + +# Regular expression matching correct argument names. Overrides argument- +# naming-style +#argument-rgx= + +# Naming style matching correct attribute names +attr-naming-style=snake_case + +# Regular expression matching correct attribute names. Overrides attr-naming- +# style +#attr-rgx= + +# Bad variable names which should always be refused, separated by a comma +bad-names=foo, + bar, + baz, + toto, + tutu, + tata + +# Naming style matching correct class attribute names +class-attribute-naming-style=any + +# Regular expression matching correct class attribute names. Overrides class- +# attribute-naming-style +#class-attribute-rgx= + +# Naming style matching correct class names +class-naming-style=any + +# Regular expression matching correct class names. Overrides class-naming-style +#class-rgx= + +# Naming style matching correct constant names +const-naming-style=UPPER_CASE + +# Regular expression matching correct constant names. Overrides const-naming- +# style +#const-rgx= + +# Minimum line length for functions/classes that require docstrings, shorter +# ones are exempt. +docstring-min-length=-1 + +# Naming style matching correct function names +function-naming-style=snake_case + +# Regular expression matching correct function names. Overrides function- +# naming-style +#function-rgx= + +# Good variable names which should always be accepted, separated by a comma +good-names=i, + j, + k, + f, + ex, + Run, + _, + x, + v + +# Include a hint for the correct naming format with invalid-name +include-naming-hint=yes + +# Naming style matching correct inline iteration names +inlinevar-naming-style=any + +# Regular expression matching correct inline iteration names. Overrides +# inlinevar-naming-style +#inlinevar-rgx= + +# Naming style matching correct method names +method-naming-style=snake_case + +# Regular expression matching correct method names. Overrides method-naming- +# style +#method-rgx= + +# Naming style matching correct module names +module-naming-style=snake_case + +# Regular expression matching correct module names. Overrides module-naming- +# style +#module-rgx= + +# Colon-delimited sets of names that determine each other's naming style when +# the name regexes allow several styles. +name-group= + +# Regular expression which should only match function or class names that do +# not require a docstring. +no-docstring-rgx=^_ + +# List of decorators that produce properties, such as abc.abstractproperty. Add +# to this list to register other decorators that produce valid properties. +property-classes=abc.abstractproperty + +# Naming style matching correct variable names +variable-naming-style=snake_case + +# Regular expression matching correct variable names. Overrides variable- +# naming-style +#variable-rgx= + + +[FORMAT] + +# Expected format of line ending, e.g. empty (any line ending), LF or CRLF. +expected-line-ending-format= + +# Regexp for a line that is allowed to be longer than the limit. +ignore-long-lines=^\s*(# )??$ + +# Number of spaces of indent required inside a hanging or continued line. +indent-after-paren=4 + +# String used as indentation unit. This is usually " " (4 spaces) or "\t" (1 +# tab). +indent-string=' ' + +# Maximum number of characters on a single line. +max-line-length=100 + +# Maximum number of lines in a module +max-module-lines=10000 + +# List of optional constructs for which whitespace checking is disabled. `dict- +# separator` is used to allow tabulation in dicts, etc.: {1 : 1,\n222: 2}. +# `trailing-comma` allows a space between comma and closing bracket: (a, ). +# `empty-line` allows space-only lines. +no-space-check=trailing-comma, + dict-separator + +# Allow the body of a class to be on the same line as the declaration if body +# contains single statement. +single-line-class-stmt=no + +# Allow the body of an if to be on the same line as the test if there is no +# else. +single-line-if-stmt=no + + +[MISCELLANEOUS] + +# List of note tags to take in consideration, separated by a comma. +notes=FIXME, + XXX + + +[VARIABLES] + +# List of additional names supposed to be defined in builtins. Remember that +# you should avoid to define new builtins when possible. +additional-builtins= + +# Tells whether unused global variables should be treated as a violation. +allow-global-unused-variables=yes + +# List of strings which can identify a callback function by name. A callback +# name must start or end with one of those strings. +callbacks=cb_, + _cb + +# A regular expression matching the name of dummy variables (i.e. expectedly +# not used). +dummy-variables-rgx=_+$|(_[a-zA-Z0-9_]*[a-zA-Z0-9]+?$)|dummy|^ignored_|^unused_ + +# Argument names that match this expression will be ignored. Default to name +# with leading underscore +ignored-argument-names=_.*|^ignored_|^unused_ + +# Tells whether we should check for unused import in __init__ files. +init-import=no + +# List of qualified module names which can have objects that can redefine +# builtins. +redefining-builtins-modules=six.moves,past.builtins,future.builtins + + +[IMPORTS] + +# Allow wildcard imports from modules that define __all__. +allow-wildcard-with-all=yes + +# Analyse import fallback blocks. This can be used to support both Python 2 and +# 3 compatible code, which means that the block might have code that exists +# only in one or another interpreter, leading to false positives when analysed. +analyse-fallback-blocks=no + +# Deprecated modules which should not be used, separated by a comma +deprecated-modules=optparse,tkinter.tix + +# Create a graph of external dependencies in the given file (report RP0402 must +# not be disabled) +ext-import-graph= + +# Create a graph of every (i.e. internal and external) dependencies in the +# given file (report RP0402 must not be disabled) +import-graph= + +# Create a graph of internal dependencies in the given file (report RP0402 must +# not be disabled) +int-import-graph= + +# Force import order to recognize a module as part of the standard +# compatibility libraries. +known-standard-library= + +# Force import order to recognize a module as part of a third party library. +known-third-party=enchant + + +[CLASSES] + +# List of method names used to declare (i.e. assign) instance attributes. +defining-attr-methods=__init__, + __new__, + setUp + +# List of member names, which should be excluded from the protected access +# warning. +exclude-protected=_asdict, + _fields, + _replace, + _source, + _make + +# List of valid names for the first argument in a class method. +valid-classmethod-first-arg=cls + +# List of valid names for the first argument in a metaclass class method. +valid-metaclass-classmethod-first-arg=mcs + + +[DESIGN] + +# Maximum number of arguments for function / method +max-args=15 + +# Maximum number of attributes for a class (see R0902). +max-attributes=7 + +# Maximum number of boolean expressions in a if statement +max-bool-expr=5 + +# Maximum number of branch for function / method body +max-branches=12 + +# Maximum number of locals for function / method body +max-locals=15 + +# Maximum number of parents for a class (see R0901). +max-parents=7 + +# Maximum number of public methods for a class (see R0904). +max-public-methods=20 + +# Maximum number of return / yield for function / method body +max-returns=6 + +# Maximum number of statements in function / method body +max-statements=50 + +# Minimum number of public methods for a class (see R0903). +min-public-methods=2 + + +[EXCEPTIONS] + +# Exceptions that will emit a warning when being caught. Defaults to +# "Exception" +overgeneral-exceptions=Exception diff --git a/PythonAPI/carla/source/holoocean/sensors.py b/PythonAPI/carla/source/holoocean/sensors.py new file mode 100644 index 0000000000..c1dbf4d5a9 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/sensors.py @@ -0,0 +1,1751 @@ +"""Definition of all of the sensor information. The UE settings for sensors are such that the motion ticks happen before the sensor ticks.""" +import json + +import numpy as np +import holoocean + +from holoocean.command import ( + RGBCameraRateCommand, + RotateSensorCommand, + CustomCommand, + SendAcousticMessageCommand, + SendOpticalMessageCommand, +) +from holoocean.exceptions import HoloOceanConfigurationException +from holoocean.lcm import SensorData + + +class HoloOceanSensor: + """Base class for a sensor + + Args: + client (:class:`~holoocean.holooceanclient.HoloOceanClient`): Client + attached to a sensor + agent_name (:obj:`str`): Name of the parent agent + agent_type (:obj:`str`): Type of the parent agent + name (:obj:`str`): Name of the sensor + config (:obj:`dict`): Configuration dictionary to pass to the engine + """ + + default_config = {} + + def __init__( + self, + client, + agent_name=None, + agent_type=None, + name="DefaultSensor", + config=None, + ): + self.name = name + self._client = client + self.agent_name = agent_name + self.agent_type = agent_type + self._buffer_name = self.agent_name + "_" + self.name + + self._sensor_data_buffer = self._client.malloc( + self._buffer_name + "_sensor_data", self.data_shape, self.dtype + ) + + self.config = {} if config is None else config + + @property + def sensor_data(self): + """Get the sensor data buffer + + Returns: + :obj:`np.ndarray` of size :obj:`self.data_shape`: Current sensor data + + """ + if self.tick_count == self.tick_every: + return self._sensor_data_buffer + else: + return None + + @property + def dtype(self): + """The type of data in the sensor + + Returns: + numpy dtype: Type of sensor data + """ + raise NotImplementedError("Child class must implement this property") + + @property + def data_shape(self): + """The shape of the sensor data + + Returns: + :obj:`tuple`: Sensor data shape + """ + raise NotImplementedError("Child class must implement this property") + + def rotate(self, rotation): + """Rotate the sensor. It will be applied in approximately three ticks. + :meth:`~holoocean.environments.HoloOceanEnvironment.step` or + :meth:`~holoocean.environments.HoloOceanEnvironment.tick`.) + + This will not persist after a call to reset(). If you want a persistent rotation for a sensor, + specify it in your scenario configuration. + + Args: + rotation (:obj:`list` of :obj:`float`): rotation for sensor (see :ref:`rotations`). + """ + command_to_send = RotateSensorCommand(self.agent_name, self.name, rotation) + self._client.command_center.enqueue_command(command_to_send) + + def reset(self): + pass + + +class DistanceTask(HoloOceanSensor): + sensor_type = "DistanceTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + +class LocationTask(HoloOceanSensor): + sensor_type = "LocationTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + +class FollowTask(HoloOceanSensor): + sensor_type = "FollowTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + +class AvoidTask(HoloOceanSensor): + sensor_type = "AvoidTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + +class CupGameTask(HoloOceanSensor): + sensor_type = "CupGameTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + def start_game(self, num_shuffles, speed=3, seed=None): + """Start the cup game and set its configuration. Do not call if the config file contains a cup task configuration + block, as it will override the configuration and cause undefined behavior. + + Args: + num_shuffles (:obj:`int`): Number of shuffles + speed (:obj:`int`): Speed of the shuffle. Works best between 1-10 + seed (:obj:`int`): Seed to rotate the cups the same way every time. If none is given, a seed will not be used. + """ + use_seed = seed is not None + if seed is None: + seed = 0 # have to pass a value + config_command = CustomCommand( + "CupGameConfig", num_params=[speed, num_shuffles, int(use_seed), seed] + ) + start_command = CustomCommand("StartCupGame") + self._client.command_center.enqueue_command(config_command) + self._client.command_center.enqueue_command(start_command) + + +class CleanUpTask(HoloOceanSensor): + sensor_type = "CleanUpTask" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [2] + + def start_task(self, num_trash, use_table=False): + """Spawn trash around the trash can. Do not call if the config file contains a clean up task configuration + block. + + Args: + num_trash (:obj:`int`): Amount of trash to spawn + use_table (:obj:`bool`, optional): If True a table will spawn next to the trash can, all trash will be on + the table, and the trash can lid will be absent. This makes the task significantly easier. If False, + all trash will spawn on the ground. Defaults to False. + """ + + if self.config is not None or self.config is not {}: + raise HoloOceanConfigurationException( + "Called CleanUpTask start_task when configuration block already \ + specified. Must remove configuration block before calling." + ) + + config_command = CustomCommand( + "CleanUpConfig", num_params=[num_trash, int(use_table)] + ) + self._client.command_center.enqueue_command(config_command) + + +class ViewportCapture(HoloOceanSensor): + """Captures what the viewport is seeing. + + The ViewportCapture is faster than the RGB camera, but there can only be one camera + and it must capture what the viewport is capturing. If performance is + critical, consider this camera instead of the RGBCamera. + + It may be useful + to position the camera with + :meth:`~holoocean.environments.HoloOceanEnvironment.teleport_camera`. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the following + options: + + - ``CaptureWidth``: Width of captured image + - ``CaptureHeight``: Height of captured image + + **THESE DIMENSIONS MUST MATCH THE VIEWPORT DIMENSTIONS** + + If you have configured the size of the viewport (``window_height/width``), you must + make sure that ``CaptureWidth/Height`` of this configuration block is set to the same + dimensions. + + The default resolution is ``1280x720``, matching the default Viewport resolution. + """ + + sensor_type = "ViewportCapture" + + def __init__( + self, client, agent_name, agent_type, name="ViewportCapture", config=None + ): + self.config = {} if config is None else config + + width = 1280 + height = 720 + + if "CaptureHeight" in self.config: + height = self.config["CaptureHeight"] + + if "CaptureWidth" in self.config: + width = self.config["CaptureWidth"] + + self.shape = (height, width, 4) + + super(ViewportCapture, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.uint8 + + @property + def data_shape(self): + return self.shape + + +class RGBCamera(HoloOceanSensor): + """Captures agent's view. + + The default capture resolution is 256x256x256x4, corresponding to the RGBA channels. + The resolution can be increased, but will significantly impact performance. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the following + options: + + - ``CaptureWidth``: Width of captured image + - ``CaptureHeight``: Height of captured image + + """ + + sensor_type = "RGBCamera" + + def __init__(self, client, agent_name, agent_type, name="RGBCamera", config=None): + self.config = {} if config is None else config + + width = 256 + height = 256 + + if "CaptureHeight" in self.config: + height = self.config["CaptureHeight"] + + if "CaptureWidth" in self.config: + width = self.config["CaptureWidth"] + + self.shape = (height, width, 4) + + super(RGBCamera, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.uint8 + + @property + def data_shape(self): + return self.shape + + def set_ticks_per_capture(self, ticks_per_capture): + """Sets this RGBCamera to capture a new frame every ticks_per_capture. + + The sensor's image will remain unchanged between captures. + + This method must be called after every call to env.reset. + + Args: + ticks_per_capture (:obj:`int`): The amount of ticks to wait between camera captures. + """ + if not isinstance(ticks_per_capture, int) or ticks_per_capture < 1: + raise HoloOceanConfigurationException( + "Invalid ticks_per_capture value " + str(ticks_per_capture) + ) + + command_to_send = RGBCameraRateCommand( + self.agent_name, self.name, ticks_per_capture + ) + self._client.command_center.enqueue_command(command_to_send) + self.tick_every = ticks_per_capture + + +class OrientationSensor(HoloOceanSensor): + """Gets the forward, right, and up vector for the agent. + Note that this is based on the sensor's frame, not the agent's frame so in the IMU socket, + it will be NED, and in COM socket it will NWU. Our provided configurations have the sensor in the IMU socket. + + Returns a 2D numpy array of + + :: + + [ [forward_x, right_x, up_x], + [forward_y, right_y, up_y], + [forward_z, right_z, up_z] ] + + """ + + sensor_type = "OrientationSensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3, 3] + + +class IMUSensor(HoloOceanSensor): + """Inertial Measurement Unit sensor. + + Returns a 2D numpy array of:: + + [ [accel_x, accel_y, accel_z], + [ang_vel_roll, ang_vel_pitch, ang_vel_yaw], + [accel_bias_x, accel_bias_y, accel_bias_z], + [ang_vel_bias_roll, ang_vel_bias_pitch, ang_vel_bias_yaw] ] + + + where the accleration components are in m/s and the angular velocity is in rad/s. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``AccelSigma``/``AccelCov``: Covariance/Std for acceleration component. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``AngVelSigma``/``AngVelCov``: Covariance/Std for angular velocity component. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``AccelBiasSigma``/``AccelCBiasov``: Covariance/Std for acceleration bias component. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``AngVelBiasSigma``/``AngVelBiasCov``: Covariance/Std for acceleration bias component. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``ReturnBias``: Whether the sensor should return the bias along with accel/ang. vel. Defaults to false. + + """ + + sensor_type = "IMUSensor" + + def __init__(self, client, agent_name, agent_type, name="IMUSensor", config=None): + self.config = {} if config is None else config + + return_bias = self.config.get("ReturnBias", False) + + if "AccelSigma" in self.config and "AccelCov" in self.config: + raise ValueError( + "Can't set both AccelSigma and AccelCov in IMUSensor, use one of them in your configuration" + ) + + if "AngVelSigma" in self.config and "AngVelCov" in self.config: + raise ValueError( + "Can't set both AngVelSigma and AngVelCov in IMUSensor, use one of them in your configuration" + ) + + if "AccelBiasSigma" in self.config and "AccelBiasCov" in self.config: + raise ValueError( + "Can't set both AccelBiasSigma and AccelBiasCov in IMUSensor, use one of them in your configuration" + ) + + if "AngVelBiasSigma" in self.config and "AngVelBiasCov" in self.config: + raise ValueError( + "Can't set both AngVelBiasSigma and AngVelBiasCov in IMUSensor, use one of them in your configuration" + ) + + self.shape = [4, 3] if return_bias else [2, 3] + + super(IMUSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class JointRotationSensor(HoloOceanSensor): + """Returns the state of the :class:`~holoocean.agents.AndroidAgent`'s or the + :class:`~holoocean.agents.HandAgent`'s joints. + + """ + + sensor_type = "JointRotationSensor" + + def __init__(self, client, agent_name, agent_type, name="RGBCamera", config=None): + if holoocean.agents.AndroidAgent.agent_type in agent_type: + # Should match AAndroid::TOTAL_DOF + self.elements = 94 + elif agent_type == holoocean.agents.HandAgent.agent_type: + # AHandAgent::TOTAL_JOINT_DOF + self.elements = 23 + else: + raise HoloOceanConfigurationException( + "Attempting to use JointRotationSensor with unsupported" + "agent type '{}'!".format(agent_type) + ) + + super(JointRotationSensor, self).__init__( + client, agent_name, agent_type, name, config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [self.elements] + + +class PressureSensor(HoloOceanSensor): + """For each joint on the :class:`~holoocean.agents.AndroidAgent` or the + :class:`~holoocean.agents.HandAgent`, returns the pressure on the + joint. + + For each joint, returns ``[x_loc, y_loc, z_loc, force]``. + + """ + + sensor_type = "PressureSensor" + + def __init__(self, client, agent_name, agent_type, name="RGBCamera", config=None): + if holoocean.agents.AndroidAgent.agent_type in agent_type: + # Should match AAndroid::NUM_JOINTS + self.elements = 48 + elif agent_type == holoocean.agents.HandAgent.agent_type: + # AHandAgent::NUM_JOINTS + self.elements = 16 + else: + raise HoloOceanConfigurationException( + "Attempting to use PressureSensor with unsupported" + "agent type '{}'!".format(agent_type) + ) + + super(PressureSensor, self).__init__( + client, agent_name, agent_type, name, config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [self.elements * (3 + 1)] + + +class RelativeSkeletalPositionSensor(HoloOceanSensor): + """Gets the position of each bone in a skeletal mesh as a quaternion. + + Returns a numpy array with four entries for each bone. + """ + + def __init__(self, client, agent_name, agent_type, name="RGBCamera", config=None): + if holoocean.agents.AndroidAgent.agent_type in agent_type: + # Should match AAndroid::NumBones + self.elements = 60 + elif agent_type == holoocean.agents.HandAgent.agent_type: + # AHandAgent::NumBones + self.elements = 17 + else: + raise HoloOceanConfigurationException( + "Attempting to use RelativeSkeletalPositionSensor with unsupported" + "agent type {}!".format(agent_type) + ) + super(RelativeSkeletalPositionSensor, self).__init__( + client, agent_name, agent_type, name, config + ) + + sensor_type = "RelativeSkeletalPositionSensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [self.elements, 4] + + +class LocationSensor(HoloOceanSensor): + """Gets the location of the agent in the world. + + Returns coordinates in ``[x, y, z]`` format (see :ref:`coordinate-system`) + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``Sigma``/``Cov``: Covariance/Std. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + + """ + + sensor_type = "LocationSensor" + + def __init__( + self, client, agent_name, agent_type, name="LocationSensor", config=None + ): + self.config = {} if config is None else config + + if "Sigma" in self.config and "Cov" in self.config: + raise ValueError( + "Can't set both Sigma and Cov in LocationSensor, use one of them in your configuration" + ) + + super(LocationSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3] + + +class RotationSensor(HoloOceanSensor): + """Gets the rotation of the agent in the world, with rotation XYZ about the fixed frame, in degrees. + + Returns ``[roll, pitch, yaw]`` (see :ref:`rotations`) + """ + + sensor_type = "RotationSensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3] + + +class VelocitySensor(HoloOceanSensor): + """Returns the x, y, and z velocity of the sensor in the global frame.""" + + sensor_type = "VelocitySensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3] + + +class DynamicsSensor(HoloOceanSensor): + """Gives all relevant information needed for implementing custom dynamics. Returns are all in the global frame and are as follows: + + [acceleration, velocity, position, angular accel., angular velocity, rpy] + + all of which are 3-vectors and rpy is roll, pitch, and yaw. This is the ONLY sensor that by default + doesn't operate in its socket, but rather in the COM. This is mostly for convenience since 99% of the + time when doing custom dynamics the information is wanted at the COM. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``UseCOM``: Whether to return data relative to the COM or the specified socket. Defaults to true. + - ``UseRPY``: Whether to return orientation as roll, pitch, yaw, or as a quaternion. Defaults to true. When true, sensor returns an 18 vector, when false sensor returns as 19 vector. Quaternion is specified with scalar as last value ie x,y,z,w. + """ + + sensor_type = "DynamicsSensor" + + def __init__( + self, client, agent_name, agent_type, name="DynamicsSensor", config=None + ): + self.config = {} if config is None else config + + use_rpy = self.config.get("UseRPY", True) + + self.shape = [18] if use_rpy else [19] + + super(DynamicsSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class CollisionSensor(HoloOceanSensor): + """Returns true if the agent is colliding with anything (including the ground).""" + + sensor_type = "CollisionSensor" + + @property + def dtype(self): + return np.bool_ + + @property + def data_shape(self): + return [1] + + +class RangeFinderSensor(HoloOceanSensor): + """Returns distances to nearest collisions in the directions specified by + the parameters. For example, if an agent had two range sensors at different + angles with 24 lasers each, the LaserDebug traces would look something like + this: + + .. image:: ../../docs/images/UAVRangeFinder.PNG + + That is, for 1 laser, you'd have 1 laser facing forward, for 3, you'd have one forward, + with the other 2 distributed evenly along the circle, at 120 degree intervals, and for + 24, you'd have a laser spaced every 15 degrees. + + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``LaserMaxDistance``: Max Distance in meters of RangeFinder. (default 10) + - ``LaserCount``: Number of lasers in sensor. Lasers are distributed evenly along the cone defined by laser angle. (default 1) + - ``LaserAngle``: Angle of elevation of lasers from origin. Measured in degrees. Straight up would be 90 degrees, down would be -90. (default 0) + - ``LaserDebug``: Show debug traces. (default false) + """ + + sensor_type = "RangeFinderSensor" + + def __init__( + self, client, agent_name, agent_type, name="RangeFinderSensor", config=None + ): + config = {} if config is None else config + self.laser_count = config["LaserCount"] if "LaserCount" in config else 1 + + super().__init__(client, agent_name, agent_type, name, config) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [self.laser_count] + + +class WorldNumSensor(HoloOceanSensor): + """Returns any numeric value from the world corresponding to a given key. This is + world specific. + + """ + + sensor_type = "WorldNumSensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [1] + + +class BallLocationSensor(WorldNumSensor): + """For the CupGame task, returns which cup the ball is underneath. + + The cups are numbered 0-2, from the agents perspective, left to right. As soon + as a swap begins, the number returned by this sensor is updated to the balls new + position after the swap ends. + + Only works in the CupGame world. + + """ + + default_config = {"Key": "BallLocation"} + + @property + def dtype(self): + return np.int8 + + +class AbuseSensor(HoloOceanSensor): + """Returns True if the agent has been abused. Abuse is calculated differently for + different agents. The Sphere and Hand agent cannot be abused. The Uav, Android, + and Turtle agents can be abused by experiencing high levels of acceleration. + The Uav is abused when its blades collide with another object, and the Turtle + agent is abused when it's flipped over. + + **Configuration** + + - ``AccelerationLimit``: Maximum acceleration the agent can endure before + being considered abused. The default depends on the agent, usually around 300 m/s^2. + + """ + + sensor_type = "AbuseSensor" + + @property + def dtype(self): + return np.int8 + + @property + def data_shape(self): + return [1] + + +######################## HOLOOCEAN CUSTOM SENSORS ########################### +# Make sure to also add your new sensor to SensorDefinition below + + +class SidescanSonar(HoloOceanSensor): + """Simulates a sidescan sonar. See :ref:`configure-octree` for more on + how to configure the octree that is used. + + The ``configuration`` block (see :ref:`configuration-block`) accepts any of + the options in the following sections. + + **Basic Configuration** + + - ``Azimuth``: Azimuth (side to side) angle visible in degrees, defaults to 170. + - ``Elevation``: Elevation angle (up and down) visible in degrees, defaults to 0.25. + - ``RangeMin``: Minimum range visible in meters, defaults to 0.5. + - ``RangeMax``: Maximum range visible in meters, defaults to 35. + - ``RangeBins``/``RangeRes``: Number of range bins of resulting image, or resolution (length in meters) of each bin. Set one or the other. Defaults to 0.05 m. + + **Noise Configuration** + + - ``AddSigma``/``AddCov``: Additive noise std/covariance from a Rayleigh distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultSigma``/``MultCov``: Multiplication noise std/covariance from a normal distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + + **Advanced Configuration** + + - ``ShowWarning``: Whether to show on screen warning about sonar computation happening. Defaults to True. + - ``AzimuthBins``/``AzimuthRes``: Number of azimuth bins of resulting image, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the OctreeMin. + - ``ElevationBins``/``ElevationRes``: Number of elevation bins used when shadowing is done, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the octree size and the min range. Should only be set if shadowing isn't working. + - ``InitOctreeRange``: Upon startup, all mid-level octrees within this distance of the agent will be created. + - ``ViewRegion``: Turns on green lines to see visible region. Defaults to False. + - ``ViewOctree``: What octree leaves to show. Less than -1 means none, -1 means all, and anything greater than or equal to 0 shows the corresponding beam index. Defaults to -10. + - ``ShadowEpsilon``: What constitutes a break between clusters when shadowing. Defaults to 4*OctreeMin. + - ``WaterDensity``: Density of water in kg/m^3. Defaults to 997. + - ``WaterSpeedSound``: Speed of sound in water in m/s. Defaults to 1480. + - ``UseApprox``: Whether to use the faster approximation of Atan2 or the slower exact implementation. Defaults to True. + + """ + + sensor_type = "SidescanSonar" + + def __init__( + self, client, agent_name, agent_type, name="SidescanSonar", config=None + ): + self.config = {} if config is None else config + + range_min = self.config.get("RangeMin", 0.5) + range_max = self.config.get("RangeMax", 35) + range_res = 0.05 + + if "RangeBins" in self.config and "RangeRes" in self.config: + raise ValueError( + "Can't set both RangeBins and RangeRes in SidescanSonar, use one of them in your configuration" + ) + elif "RangeBins" in self.config: + range_bins = self.config["RangeBins"] + elif "RangeRes" in self.config: + range_bins = int((range_max - range_min) // self.config["RangeRes"]) + else: + range_bins = int((range_max - range_min) // range_res) + + if "AzimuthBins" in self.config and "AzimuthRes" in self.config: + raise ValueError( + "Can't set both AzimuthBins and AzimuthRes in SidescanSonar, use one of them in your configuration" + ) + + if "ElevationBins" in self.config and "ElevationRes" in self.config: + raise ValueError( + "Can't set both ElevationBins and ElevationRes in SidescanSonar, use one of them in your configuration" + ) + + if "AddSigma" in self.config and "AddCov" in self.config: + raise ValueError( + "Can't set both AddSigma and AddCov in SidescanSonar, use one of them in your configuration" + ) + + if "MultSigma" in self.config and "MultCov" in self.config: + raise ValueError( + "Can't set both MultSigma and MultCov in SidescanSonar, use one of them in your configuration" + ) + + # Ensure shape of python variable matches what will be sent from the c++ side + self.shape = [range_bins] + + super(SidescanSonar, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class ImagingSonar(HoloOceanSensor): + """Simulates an imaging sonar. See :ref:`configure-octree` for more on + how to configure the octree that is used. + + The ``configuration`` block (see :ref:`configuration-block`) accepts any of + the options in the following sections. + + **Basic Configuration** + + - ``Azimuth``: Azimuth (side to side) angle visible in degrees, defaults to 120. + - ``Elevation``: Elevation angle (up and down) visible in degrees, defaults to 20. + - ``RangeMin``: Minimum range visible in meters, defaults to 0.1. + - ``RangeMax``: Maximum range visible in meters, defaults to 10. + - ``RangeBins``/``RangeRes``: Number of range bins of resulting image, or resolution (length in meters) of each bin. Set one or the other. Defaults to 512 bins. + - ``AzimuthBins``/``AzimuthRes``: Number of azimuth bins of resulting image, or resolution (length in degrees) of each bin. Set one or the other. Defaults to 512 bins. + + **Noise Configuration** + + - ``AddSigma``/``AddCov``: Additive noise std/covariance from a Rayleigh distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultSigma``/``MultCov``: Multiplication noise std/covariance from a normal distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultiPath``: Whether to compute multipath or not. Defaults to False. + - ``ClusterSize``: Size of cluster when multipath is enabled. Defaults to 5. + - ``ScaleNoise``: Whether to scale the returned intensities or not. Defaults to False. + - ``AzimuthStreaks``: What sort of azimuth artifacts to introduce. -1 is a removal artifact, 0 is no artifact, and 1 is increased gain artifact. Defaults to 0. + - ``RangeSigma``: Additive noise std from an exponential distribution that will be added to the range measurements, and the intensities will be scaled by the pdf. Needs to be a float. Defaults to 0, or off. + + **Advanced Configuration** + + - ``ShowWarning``: Whether to show on screen warning about sonar computation happening. Defaults to True. + - ``ElevationBins``/``ElevationRes``: Number of elevation bins used when shadowing is done, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the octree size and the min/max range. Should only be set if shadowing isn't working. + - ``InitOctreeRange``: Upon startup, all mid-level octrees within this distance of the agent will be created. + - ``ViewRegion``: Turns on green lines to see visible region. Defaults to False. + - ``ViewOctree``: What octree leaves to show. Less than -1 means none, -1 means all, and anything greater than or equal to 0 shows the corresponding beam index. Defaults to -10. + - ``ShadowEpsilon``: What constitutes a break between clusters when shadowing. Defaults to 4*OctreeMin. + - ``WaterDensity``: Density of water in kg/m^3. Defaults to 997. + - ``WaterSpeedSound``: Speed of sound in water in m/s. Defaults to 1480. + - ``UseApprox``: Whether to use the faster approximation of Atan2 or the slower exact implementation. Defaults to True. + + """ + + sensor_type = "ImagingSonar" + + def __init__( + self, client, agent_name, agent_type, name="ImagingSonar", config=None + ): + self.config = {} if config is None else config + + b_range = 512 + b_azimuth = 512 + min_range = self.config.get("RangeMin", 0.1) + max_range = self.config.get("RangeMax", 10) + azimuth = self.config.get("Azimuth", 120) + + if "RangeBins" in self.config and "RangeRes" in self.config: + raise ValueError( + "Can't set both RangeBins and RangeRes in ImagingSonar, use one of them in your configuration" + ) + elif "RangeBins" in self.config: + b_range = self.config["RangeBins"] + elif "RangeRes" in self.config: + b_range = int((max_range - min_range) // self.config["RangeRes"]) + + if "AzimuthBins" in self.config and "AzimuthRes" in self.config: + raise ValueError( + "Can't set both AzimuthBins and AzimuthRes in ImagingSonar, use one of them in your configuration" + ) + elif "AzimuthBins" in self.config: + b_azimuth = self.config["AzimuthBins"] + elif "AzimuthRes" in self.config: + b_azimuth = int(azimuth // self.config["AzimuthRes"]) + + if "ElevationBins" in self.config and "ElevationRes" in self.config: + raise ValueError( + "Can't set both ElevationBins and ElevationRes in ImagingSonar, use one of them in your configuration" + ) + + if "AddSigma" in self.config and "AddCov" in self.config: + raise ValueError( + "Can't set both AddSigma and AddCov in ImagingSonar, use one of them in your configuration" + ) + + if "MultSigma" in self.config and "MultCov" in self.config: + raise ValueError( + "Can't set both MultSigma and MultCov in ImagingSonar, use one of them in your configuration" + ) + + self.shape = (b_range, b_azimuth) + + super(ImagingSonar, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class SinglebeamSonar(HoloOceanSensor): + """Simulates an echosounder, which is a sonar sensor with a single cone shaped beam. See :ref:`configure-octree` for more on + how to configure the octree that is used. + + Returns a 1D numpy array of the average intensities held in each range bin of the sensor. The length of the array is specified by the number of range bins chosen for the sensor. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the following + options: + + The ``configuration`` block (see :ref:`configuration-block`) accepts any of + the options in the following sections. + + **Basic Configuration** + + - ``OpeningAngle``: Opening angle of the cone visible in degrees, defaults to 30. In this documentation, the opening angle would be 2 times the semi-vertical angle of the cone. + - ``RangeMin``: Minimum range visible in meters, defaults to 0.5. + - ``RangeMax``: Maximum range visible in meters, defaults to 10. + - ``RangeBins``/``RangeRes``: Number of range bins of resulting image, or resolution (length in meters) of each bin. Set one or the other. Defaults to 200 bins. + + **Noise Configuration** + + - ``AddSigma``/``AddCov``: Additive noise std/covariance from a Rayleigh distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultSigma``/``MultCov``: Multiplication noise std/covariance from a normal distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``RangeSigma``: Additive noise std from an exponential distribution that will be added to the range measurements. Needs to be a float. Defaults to 0/off. + + **Advanced Configuration** + + - ``ShowWarning``: Whether to show on screen warning about sonar computation happening. Defaults to True. + - ``OpeningAngleBins``/``OpeningAngleRes``: Number of OpeningAngle bins used when shadowing is done, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the octree size and the min/max range. Should only be set if shadowing isn't working. + - ``CentralAngleBins``/``CentralAngleRes``: Number of CentralAngle bins used when shadowing is done, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the octree size and the min/max range. Should only be set if shadowing isn't working. + - ``InitOctreeRange``: Upon startup, all mid-level octrees within this distance of the agent will be created. + - ``ViewRegion``: Turns on green lines to see visible region. Defaults to False. + - ``ViewOctree``: What octree leaves to show. Less than -1 means none, -1 means all, and anything greater than or equal to 0 shows the corresponding beam index. Defaults to -10. + - ``ShadowEpsilon``: What constitutes a break between clusters when shadowing. Defaults to 4*OctreeMin. + - ``WaterDensity``: Density of water in kg/m^3. Defaults to 997. + - ``WaterSpeedSound``: Speed of sound in water in m/s. Defaults to 1480. + - ``UseApprox``: Whether to use the faster approximation of Atan2 or the slower exact implementation. Defaults to True. + """ + + sensor_type = "SinglebeamSonar" + + def __init__( + self, client, agent_name, agent_type, name="SinglebeamSonar", config=None + ): + self.config = {} if config is None else config + + # default range for bins + b_range = 200 + + if "BinsRange" in self.config: + b_range = self.config["BinsRange"] + + b_range = 200 + min_range = self.config.get("RangeMin", 0.5) + max_range = self.config.get("RangeMax", 10) + + if "RangeBins" in self.config and "RangeRes" in self.config: + raise ValueError( + "Can't set both RangeBins and RangeRes in SinglebeamSonar, use one of them in your configuration" + ) + elif "RangeBins" in self.config: + b_range = self.config["RangeBins"] + elif "RangeRes" in self.config: + b_range = int((max_range - min_range) // self.config["RangeRes"]) + + if "OpeningAngleBins" in self.config and "OpeningAngleRes" in self.config: + raise ValueError( + "Can't set both OpeningAngleBins and OpeningAngleRes in SinglebeamSonar, use one of them in your configuration" + ) + + if "CentralAngleBins" in self.config and "CentralAngleRes" in self.config: + raise ValueError( + "Can't set both CentralAngleBins and CentralAngleRes in SinglebeamSonar, use one of them in your configuration" + ) + + if "AddSigma" in self.config and "AddCov" in self.config: + raise ValueError( + "Can't set both AddSigma and AddCov in SinglebeamSonar, use one of them in your configuration" + ) + + if "MultSigma" in self.config and "MultCov" in self.config: + raise ValueError( + "Can't set both MultSigma and MultCov in SinglebeamSonar, use one of them in your configuration" + ) + + self.shape = [b_range] + + super(SinglebeamSonar, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class ProfilingSonar(ImagingSonar): + """Simulates a multibeam profiling sonar. This is largely based off of the imaging sonar (:class:`~holoocean.sensors.ImagingSonar`), just with + different defaults. See :ref:`configure-octree` for more on how to configure the octree that is used. + + The ``configuration`` block (see :ref:`configuration-block`) accepts any of + the options in the following sections. + + **Basic Configuration** + + - ``Azimuth``: Azimuth (side to side) angle visible in degrees, defaults to 120. + - ``Elevation``: Elevation angle (up and down) visible in degrees, defaults to 1. + - ``RangeMin``: Minimum range visible in meters, defaults to 0.5. + - ``RangeMax``: Maximum range visible in meters, defaults to 75. + - ``RangeBins``/``RangeRes``: Number of range bins of resulting image, or resolution (length in meters) of each bin. Set one or the other. Defaults to 750 bins. + - ``AzimuthBins``/``AzimuthRes``: Number of azimuth bins of resulting image, or resolution (length in degrees) of each bin. Set one or the other. Defaults to 480 bins. + + **Noise Configuration** + + - ``AddSigma``/``AddCov``: Additive noise std/covariance from a Rayleigh distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultSigma``/``MultCov``: Multiplication noise std/covariance from a normal distribution. Needs to be a float. Set one or the other. Defaults to 0, or off. + - ``MultiPath``: Whether to compute multipath or not. Defaults to False. + - ``ClusterSize``: Size of cluster when multipath is enabled. Defaults to 5. + - ``ScaleNoise``: Whether to scale the returned intensities or not. Defaults to False. + - ``AzimuthStreaks``: What sort of azimuth artifacts to introduce. -1 is a removal artifact, 0 is no artifact, and 1 is increased gain artifact. Defaults to 0. + + **Advanced Configuration** + + - ``ShowWarning``: Whether to show on screen warning about sonar computation happening. Defaults to True. + - ``ElevationBins``/``ElevationRes``: Number of elevation bins used when shadowing is done, or resolution (length in degrees) of each bin. Set one or the other. By default this is computed based on the octree size and the min/max range. Should only be set if shadowing isn't working. + - ``InitOctreeRange``: Upon startup, all mid-level octrees within this distance of the agent will be created. + - ``ViewRegion``: Turns on green lines to see visible region. Defaults to False. + - ``ViewOctree``: What octree leaves to show. Less than -1 means none, -1 means all, and anything greater than or equal to 0 shows the corresponding beam index. Defaults to -10. + - ``ShadowEpsilon``: What constitutes a break between clusters when shadowing. Defaults to 4*OctreeMin. + - ``WaterDensity``: Density of water in kg/m^3. Defaults to 997. + - ``WaterSpeedSound``: Speed of sound in water in m/s. Defaults to 1480. + - ``UseApprox``: Whether to use the faster approximation of Atan2 or the slower exact implementation. Defaults to True. + + """ + + sensor_type = "ProfilingSonar" + + def __init__( + self, client, agent_name, agent_type, name="ProfilingSonar", config=None + ): + if "RangeMin" not in config: + config["RangeMin"] = 0.5 + + if "RangeMax" not in config: + config["RangeMax"] = 75 + + if "RangeBins" not in config and "RangeRes" not in config: + config["RangeBins"] = 750 + + if "AzimuthBins" not in config and "AzimuthRes" not in config: + config["AzimuthBins"] = 480 + + super(ProfilingSonar, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + +class DVLSensor(HoloOceanSensor): + """Doppler Velocity Log Sensor. + + Returns a 1D numpy array of:: + + [velocity_x, velocity_y, velocity_z, range_x_forw, range_y_forw, range_x_back, range_y_back] + + + With the range potentially not returning if ``ReturnRange`` is set to false. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``Elevation``: Angle of each acoustic beam off z-axis pointing down. Only used for noise/visualization. Defaults to 22.5 degrees => 45 field of view. + - ``DebugLines``: Whether to show lines of each beam. Defaults to false. + - ``VelSigma``/``VelCov``: Covariance/Std to be applied to each beam velocity. Can be scalar, 4-vector or 4x4-matrix. Set one or the other. Defaults to 0 => no noise. + - ``ReturnRange``: Boolean of whether range of beams should also be returned. Defaults to true. + - ``MaxRange``: Maximum range that can be returned by the beams. + - ``RangeSigma``/``RangeCov``: Covariance/Std to be applied to each beam range. Can be scalar, 4-vector or 4x4-matrix. Set one or the other. Defaults to 0 => no noise. + + """ + + sensor_type = "DVLSensor" + + def __init__(self, client, agent_name, agent_type, name="DVLSensor", config=None): + self.config = {} if config is None else config + + return_range = self.config.get("ReturnRange", True) + + if "VelSigma" in self.config and "VelCov" in self.config: + raise ValueError( + "Can't set both VelSigma and VelCov in DVLSensor, use one of them in your configuration" + ) + + if "RangeSigma" in self.config and "RangeCov" in self.config: + raise ValueError( + "Can't set both RangeSigma and RangeCov in DVLSensor, use one of them in your configuration" + ) + + self.shape = [7] if return_range else [3] + + super(DVLSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return self.shape + + +class DepthSensor(HoloOceanSensor): + """Pressure/Depth Sensor. + + Returns a 1D numpy array of:: + + [position_z] + + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``Sigma``/``Cov``: Covariance/Std to be applied, a scalar. Defaults to 0 => no noise. + + """ + + sensor_type = "DepthSensor" + + def __init__(self, client, agent_name, agent_type, name="DepthSensor", config=None): + self.config = {} if config is None else config + + if "Sigma" in self.config and "Cov" in self.config: + raise ValueError( + "Can't set both Sigma and Cov in DepthSensor, use one of them in your configuration" + ) + + super(DepthSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [1] + + +class GPSSensor(HoloOceanSensor): + """Gets the location of the agent in the world if the agent is close enough to the surface. + + Returns coordinates in ``[x, y, z]`` format (see :ref:`coordinate-system`) + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``Sigma``/``Cov``: Covariance/Std of measurement. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``Depth``: How deep in the water we can still receive GPS messages in meters. Defaults to 2m. + - ``DepthSigma``/``DepthCov``: Covariance/Std of depth. Must be a scalar. Set one or the other. Defaults to 0 => no noise. + + """ + + sensor_type = "GPSSensor" + + def __init__(self, client, agent_name, agent_type, name="GPSSensor", config=None): + self.config = {} if config is None else config + + if "Sigma" in self.config and "Cov" in self.config: + raise ValueError( + "Can't set both Sigma and Cov in GPSSensor, use one of them in your configuration" + ) + + if "DepthSigma" in self.config and "DepthCov" in self.config: + raise ValueError( + "Can't set both DepthSigma and DepthCov in GPSSensor, use one of them in your configuration" + ) + + super(GPSSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3] + + @property + def sensor_data(self): + if ( + ~np.any(np.isnan(self._sensor_data_buffer)) + and self.tick_count == self.tick_every + ): + return self._sensor_data_buffer + else: + return None + + +class MagnetometerSensor(HoloOceanSensor): + """Gets the global x-axis (or given vector) in the local frame. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``Sigma``/``Cov``: Covariance/Std of measurement. Can be scalar, 3-vector or 3x3-matrix. Set one or the other. Defaults to 0 => no noise. + - ``MagneticVector``: The given 3-vector to measure in the global frame. Defaults to [1,0,0]. + + """ + + sensor_type = "MagnetometerSensor" + + def __init__( + self, client, agent_name, agent_type, name="MagnetometerSensor", config=None + ): + self.config = {} if config is None else config + + if "Sigma" in self.config and "Cov" in self.config: + raise ValueError( + "Can't set both Sigma and Cov in MagnetometerSensor, use one of them in your configuration" + ) + + super(MagnetometerSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [3] + + +class PoseSensor(HoloOceanSensor): + """Gets the forward, right, and up vector for the agent. + Note that this is based on the sensor's frame, not the agent's frame so in the IMU socket, + it will be NED, and in COM socket it will NWU. Our provided configurations have the sensor in the IMU socket. + + Returns a 2D numpy array of + + :: + + [ [R, p], + [0, 1] ] + + where R is the rotation matrix (See OrientationSensor) and p is the robot world location (see LocationSensor) + """ + + sensor_type = "PoseSensor" + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [4, 4] + + +class AcousticBeaconSensor(HoloOceanSensor): + """Acoustic Beacon Sensor. Can send message to other beacon from the :meth:`~holoocean.HoloOceanEnvironment.send_acoustic_message` command. + + Returning array depends on sent message type. Note received message will be delayed due to time of + acoustic wave traveling. Possibly message types are, with ϕ representing the azimuth, ϴ + elevation, r range, and d depth in water, + + - ``OWAY``: One way message that sends ``["OWAY", from_sensor, payload]`` + - ``OWAYU``: One way message that sends ``["OWAYU", from_sensor, payload, ϕ, ϴ]`` + - ``MSG_REQ``: Requests a return message of MSG_RESP and sends ``["MSG_REQ", from_sensor, payload]`` + - ``MSG_RESP``: Return message that sends ``["MSG_RESP", from_sensor, payload]`` + - ``MSG_REQU``: Requests a return message of MSG_RESPU and sends ``["MSG_REQU", from_sensor, payload, ϕ, ϴ]`` + - ``MSG_RESPU``: Return message that sends ``["MSG_RESPU", from_sensor, payload, ϕ, ϴ, r]`` + - ``MSG_REQX``: Requests a return message of MSG_RESPX and sends ``["MSG_REQX", from_sensor, payload, ϕ, ϴ, d]`` + - ``MSG_RESPX``: Return message that sends ``["MSG_RESPX", from_sensor, payload, ϕ, ϴ, r, d]`` + + These messages types are based on the `Blueprint Subsea SeaTrac X150 `_ + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``id``: Id of this sensor. If not given, they are numbered sequentially. + - ``CheckVisible``: Whether to check for linear line of sight to vehicle when verify message will be received. Used when sending data. Defaults to false. + - ``MaxDistance``: Max Distance in meters of AcousticBeacon. Used when sending data. Defaults to no max distance. + - ``DistanceSigma``/``DistanceCov``: Determines the standard deviation/covariance of the noise on MaxDistance. Must be scalar value. (default 0 => no noise) + """ + + sensor_type = "AcousticBeaconSensor" + instances = dict() + + def __init__( + self, client, agent_name, agent_type, name="AcousticBeaconSensor", config=None + ): + self.sending_to = [] + + # assign an id + # TODO This could possibly assign a later to be used id + # For safety either give all beacons id's or none of them + curr_ids = set(i.id for i in self.__class__.instances.values()) + if "id" in config and config["id"] not in curr_ids: + self.id = config["id"] + elif len(curr_ids) == 0: + self.id = 0 + else: + all_ids = set(range(max(curr_ids) + 2)) + self.id = min(all_ids - curr_ids) + + # keep running list of all beacons + self.__class__.instances[self.id] = self + + super(AcousticBeaconSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + @property + def dtype(self): + return np.float32 + + @property + def data_shape(self): + return [4] + + @property + def status(self): + if len(self.sending_to) == 0: + return "Idle" + else: + return "Transmitting" + + def send_message(self, id_to, msg_type, msg_data): + # Clean out id_to parameters + if id_to == -1 or id_to == "all": + id_to = list(self.__class__.instances.keys()) + id_to.remove(self.id) + if isinstance(id_to, int): + id_to = [id_to] + + # Don't transmit if a message is still waiting to be received + if self.status == "Transmitting": + print(f"Beacon {self.id} is still transmitting") + # otherwise transmit + else: + for i in id_to: + beacon = self.__class__.instances[i] + command = SendAcousticMessageCommand( + self.agent_name, self.name, beacon.agent_name, beacon.name + ) + self._client.command_center.enqueue_command(command) + + self.sending_to.append(i) + + beacon.msg_data = msg_data + beacon.msg_type = msg_type + + @property + def sensor_data(self): + if ~np.any(np.isnan(self._sensor_data_buffer)): + # get all beacons sending messages + sending = [ + i + for i, val in self.__class__.instances.items() + if val.status == "Transmitting" + ] + + # make sure exactly one person is sending messages + if len(sending) != 1: + data = None + # reset all sending beacons since they got muddled + for i in sending: + self.__class__.instances[i].sending_to = [] + + # If the message failed to be received b/c of obstacles + elif np.all(self._sensor_data_buffer == -1): + data = None + self.__class__.instances[sending[0]].sending_to = [] + + # otherwise parse through type + else: + from_sensor = sending[0] + # stop sending to this beacon + self.__class__.instances[from_sensor].sending_to.remove(self.id) + + data = [self.msg_type, from_sensor, self.msg_data] + + phi, theta, dist, depth = self._sensor_data_buffer + + if self.msg_type == "OWAY": + pass + elif self.msg_type == "OWAYU": + data.extend([phi, theta]) + elif self.msg_type == "MSG_REQ": + self.send_message(from_sensor, "MSG_RESP", None) + elif self.msg_type == "MSG_RESP": + pass + elif self.msg_type == "MSG_REQU": + self.send_message(from_sensor, "MSG_RESPU", None) + data.extend([phi, theta]) + elif self.msg_type == "MSG_RESPU": + data.extend([phi, theta, dist]) + elif self.msg_type == "MSG_REQX": + self.send_message(from_sensor, "MSG_RESPX", None) + data.extend([phi, theta, depth]) + elif self.msg_type == "MSG_RESPX": + data.extend([phi, theta, dist, depth]) + else: + raise ValueError("Invalid Acoustic MSG type") + + # reset buffer + self.msg_data = None + self.msg_type = None + + return data + + else: + return None + + def reset(self): + self.__class__.instances = dict() + + +class OpticalModemSensor(HoloOceanSensor): + """Handles communication between agents using an optical modem. Can send message to other modem from the :meth:`~holoocean.HoloOceanEnvironment.send_optical_message` command. + + **Configuration** + + The ``configuration`` block (see :ref:`configuration-block`) accepts the + following options: + + - ``id``: Id of this sensor. If not given, they are numbered sequentially. + - ``MaxDistance``: Max Distance in meters of OpticalModem. Used when sending data. (default 50) + - ``DistanceSigma``/``DistanceCov``: Determines the standard deviation/covariance of the noise on MaxDistance. Must be scalar value. (default 0 => no noise) + - ``LaserAngle``: Angle of lasers from origin. Measured in degrees. Used when sending data. (default 60) + - ``AngleSigma``/``AngleCov``: Determines the standard deviation of the noise on LaserAngle. Must be scalar value. (default 0 => no noise) + - ``LaserDebug``: Show debug traces. (default false) + - ``DebugNumSides``: Number of sides on the debug cone. (default 72) + + """ + + sensor_type = "OpticalModemSensor" + instances = dict() + + def __init__( + self, client, agent_name, agent_type, name="OpticalModemSensor", config=None + ): + self.config = {} if config is None else config + + if "DistanceSigma" in self.config and "DistanceCov" in self.config: + raise ValueError( + "Can't set both DistanceSigma and DistanceCov in OpticalModemSensor, use one of them in your configuration" + ) + + if "AngleSigma" in self.config and "AngleCov" in self.config: + raise ValueError( + "Can't set both AngleSigma and AngleCov in OpticalModemSensor, use one of them in your configuration" + ) + + self.sending_to = [] + + # assign an id + # TODO This could possibly assign a later to be used id + # For safety either give all beacons id's or none of them + curr_ids = set(i.id for i in self.__class__.instances.values()) + if "id" in config and config["id"] not in curr_ids: + self.id = config["id"] + elif len(curr_ids) == 0: + self.id = 0 + else: + all_ids = set(range(max(curr_ids) + 2)) + self.id = min(all_ids - curr_ids) + + # keep running list of all beacons + self.__class__.instances[self.id] = self + + super(OpticalModemSensor, self).__init__( + client, agent_name, agent_type, name=name, config=config + ) + + def send_message(self, id_to, msg_data): + # Clean out id_to parameters + if id_to == -1 or id_to == "all": + id_to = list(self.__class__.instances.keys()) + id_to.remove(self.id) + if isinstance(id_to, int): + id_to = [id_to] + + for i in id_to: + modem = self.__class__.instances[i] + command = SendOpticalMessageCommand( + self.agent_name, self.name, modem.agent_name, modem.name + ) + self._client.command_center.enqueue_command(command) + + modem.msg_data = msg_data + + @property + def sensor_data(self): + if len(self._sensor_data_buffer) > 0 and self._sensor_data_buffer: + data = self.msg_data + else: + data = None + + # reset buffer + self.msg_data = None + return data + + @property + def dtype(self): + return np.bool_ + + @property + def data_shape(self): + return [1] + + def reset(self): + self.__class__.instances = dict() + + +###################################################################################### +class SensorDefinition: + """A class for new sensors and their parameters, to be used for adding new sensors. + + Args: + agent_name (:obj:`str`): The name of the parent agent. + agent_type (:obj:`str`): The type of the parent agent + sensor_name (:obj:`str`): The name of the sensor. + sensor_type (:obj:`str` or :class:`HoloOceanSensor`): The type of the sensor. + socket (:obj:`str`, optional): The name of the socket to attach sensor to. + location (Tuple of :obj:`float`, optional): ``[x, y, z]`` coordinates to place sensor + relative to agent (or socket) (see :ref:`coordinate-system`). + rotation (Tuple of :obj:`float`, optional): ``[roll, pitch, yaw]`` to rotate sensor + relative to agent (see :ref:`rotations`) + config (:obj:`dict`): Configuration dictionary for the sensor, to pass to engine + """ + + _sensor_keys_ = { + "RGBCamera": RGBCamera, + "DistanceTask": DistanceTask, + "LocationTask": LocationTask, + "FollowTask": FollowTask, + "AvoidTask": AvoidTask, + "CupGameTask": CupGameTask, + "CleanUpTask": CleanUpTask, + "ViewportCapture": ViewportCapture, + "OrientationSensor": OrientationSensor, + "IMUSensor": IMUSensor, + "JointRotationSensor": JointRotationSensor, + "RelativeSkeletalPositionSensor": RelativeSkeletalPositionSensor, + "LocationSensor": LocationSensor, + "RotationSensor": RotationSensor, + "VelocitySensor": VelocitySensor, + "DynamicsSensor": DynamicsSensor, + "PressureSensor": PressureSensor, + "CollisionSensor": CollisionSensor, + "RangeFinderSensor": RangeFinderSensor, + "WorldNumSensor": WorldNumSensor, + "BallLocationSensor": BallLocationSensor, + "AbuseSensor": AbuseSensor, + "DVLSensor": DVLSensor, + "PoseSensor": PoseSensor, + "AcousticBeaconSensor": AcousticBeaconSensor, + "DepthSensor": DepthSensor, + "OpticalModemSensor": OpticalModemSensor, + "ImagingSonar": ImagingSonar, + "SidescanSonar": SidescanSonar, + "ProfilingSonar": ProfilingSonar, + "GPSSensor": GPSSensor, + "MagnetometerSensor": MagnetometerSensor, + "SinglebeamSonar": SinglebeamSonar, + } + + # Sensors that need timeout turned off + _sonar_sensors = [ + "ImagingSonar", + "ProfilingSonar", + "SidescanSonar", + "SinglebeamSonar", + ] + + # Sensors that are ticked at their rate on the C++ too + # Generally sensors with a heavy computational cost + _heavy_sensors = _sonar_sensors + ["RGBCamera"] + + def get_config_json_string(self): + """Gets the configuration dictionary as a string ready for transport + + Returns: + (:obj:`str`): The configuration as an escaped json string + + """ + param_str = json.dumps(self.config) + # Prepare configuration string for transport to the engine + param_str = param_str.replace('"', '\\"') + return param_str + + def __init__( + self, + agent_name, + agent_type, + sensor_name, + sensor_type, + socket="", + location=(0, 0, 0), + rotation=(0, 0, 0), + config=None, + existing=False, + lcm_channel=None, + tick_every=None, + ): + self.agent_name = agent_name + self.agent_type = agent_type + self.sensor_name = sensor_name + + if isinstance(sensor_type, str): + self.type = SensorDefinition._sensor_keys_[sensor_type] + else: + self.type = sensor_type + + self.socket = socket + self.location = location + self.rotation = rotation + self.config = self.type.default_config if config is None else config + # hacky way to get heavy sensors to capture lined up with python rate + if sensor_type in SensorDefinition._heavy_sensors: + self.config["TicksPerCapture"] = tick_every + self.existing = existing + + if lcm_channel is not None: + self.lcm_msg = SensorData(sensor_type, lcm_channel) + else: + self.lcm_msg = None + self.tick_every = tick_every + + +class SensorFactory: + """Given a sensor definition, constructs the appropriate HoloOceanSensor object.""" + + @staticmethod + def _default_name(sensor_class): + return sensor_class.sensor_type + + @staticmethod + def build_sensor(client, sensor_def): + """Constructs a given sensor associated with client + + Args: + client (:obj:`str`): Name of the agent this sensor is attached to + sensor_def (:class:`SensorDefinition`): Sensor definition to construct + + Returns: + + """ + if sensor_def.sensor_name is None: + sensor_def.sensor_name = SensorFactory._default_name(sensor_def.type) + + result = sensor_def.type( + client, + sensor_def.agent_name, + sensor_def.agent_type, + sensor_def.sensor_name, + config=sensor_def.config, + ) + + # TODO: Make this part of the constructors rather than hacking it on + # Wanted to make sure this is what we want before making large changes + result.lcm_msg = sensor_def.lcm_msg + result.tick_every = sensor_def.tick_every + result.tick_count = sensor_def.tick_every + + return result diff --git a/PythonAPI/carla/source/holoocean/shmem.py b/PythonAPI/carla/source/holoocean/shmem.py new file mode 100644 index 0000000000..43519d2a7b --- /dev/null +++ b/PythonAPI/carla/source/holoocean/shmem.py @@ -0,0 +1,71 @@ +"""Shared memory with memory mapping""" +import ctypes +import mmap +import os +from functools import reduce + +import numpy as np + +from holoocean.exceptions import HoloOceanException + + +class Shmem: + """Implementation of shared memory + + + Args: + name (:obj:`str`): Name the points to the beginning of the shared memory block + shape (:obj:`int`): Shape of the memory block + dtype (type, optional): data type of the shared memory. Defaults to np.float32 + uuid (:obj:`str`, optional): UUID of the memory block. Defaults to "" + """ + _numpy_to_ctype = { + np.float32: ctypes.c_float, + np.uint8: ctypes.c_uint8, + np.bool_: ctypes.c_bool, + np.byte: ctypes.c_byte + } + + def __init__(self, name, shape, dtype=np.float32, uuid=""): + self.shape = shape + self.dtype = dtype + size = reduce(lambda x, y: x * y, shape) + size_bytes = np.dtype(dtype).itemsize * size + + self._mem_path = None + self._mem_pointer = None + if os.name == "nt": + self._mem_path = "/HOLODECK_MEM" + uuid + "_" + name + self._mem_pointer = mmap.mmap(0, size_bytes, self._mem_path) + elif os.name == "posix": + self._mem_path = "/dev/shm/HOLODECK_MEM" + uuid + "_" + name + f = os.open(self._mem_path, os.O_CREAT | os.O_TRUNC | os.O_RDWR) + self._mem_file = f + os.ftruncate(f, size_bytes) + os.fsync(f) + + # TODO - I think we are leaking a file object here. Unfortunately, we + # can't just .close() it since numpy acquires a reference to it + # below and I can't find a way to release it in __linux_unlink__() + self._mem_pointer = mmap.mmap(f, size_bytes) + else: + raise HoloOceanException("Currently unsupported os: " + os.name) + + self.np_array = np.ndarray(shape, dtype=dtype) + self.np_array.data = (Shmem._numpy_to_ctype[dtype] * size).from_buffer(self._mem_pointer) + + def unlink(self): + """unlinks the shared memory""" + if os.name == "posix": + self.__linux_unlink__() + elif os.name == "nt": + self.__windows_unlink__() + else: + raise HoloOceanException("Currently unsupported os: " + os.name) + + def __linux_unlink__(self): + os.close(self._mem_file) + os.remove(self._mem_path) + + def __windows_unlink__(self): + pass diff --git a/PythonAPI/carla/source/holoocean/spaces.py b/PythonAPI/carla/source/holoocean/spaces.py new file mode 100644 index 0000000000..c36a6702d8 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/spaces.py @@ -0,0 +1,122 @@ +"""Contains action space definitions""" +import numpy as np + + +class ActionSpace: + """Abstract ActionSpace class. + + Parameters: + shape (:obj:`list` of :obj:`int`): The shape of data that should be input to step or tick. + buffer_shape (:obj:`list` of :obj:`int`, optional): The shape of the data that will be + written to the shared memory. + + Only use this when it is different from shape. + """ + def __init__(self, shape, buffer_shape=None): + super(ActionSpace, self).__init__() + self._shape = shape + self.buffer_shape = buffer_shape or shape + + def sample(self): + """Sample from the action space. + + Returns: + (:obj:`np.ndarray`): A valid command to be input to step or tick. + """ + raise NotImplementedError("Must be implemented by child class") + + @property + def shape(self): + """Get the shape of the action space. + + Returns: + (:obj:`list` of :obj:`int`): The shape of the action space. + """ + return self._shape + + def get_low(self): + """The minimum value(s) for the action space. + + Returns: + (:obj:`list` of :obj:`float` or :obj:`float`): the action space's minimum value(s) + """ + raise NotImplementedError('Must be implemented by the child class') + + def get_high(self): + """The maximum value(s) for the action space. + + Returns: + (:obj:`list` of :obj:`float` or :obj:`float`): the action space's maximum value(s) + """ + raise NotImplementedError('Must be implemented by the child class') + + +class ContinuousActionSpace(ActionSpace): + """Action space that takes floating point inputs. + + Parameters: + shape (:obj:`list` of :obj:`int`): The shape of data that should be input to step or tick. + sample_fn (function, optional): A function that takes a shape parameter and outputs a + sampled command. + low (:obj:`list` of :obj:`float` or :obj:`float`): the low value(s) for the action space. Can be a scalar or an array + high (:obj:`list` of :obj:`float` or :obj:`float`): the high value(s) for the action space. Cand be a scalar or an array + + If this is not given, it will default to sampling from a unit gaussian. + buffer_shape (:obj:`list` of :obj:`int`, optional): The shape of the data that will be + written to the shared memory. + + Only use this when it is different from ``shape``. + """ + def __init__(self, shape, low=None, high=None, sample_fn=None, buffer_shape=None): + super(ContinuousActionSpace, self).__init__(shape, buffer_shape=buffer_shape) + self.sample_fn = sample_fn or ContinuousActionSpace._default_sample_fn + self._low = low + self._high = high + + def get_low(self): + return self._low + + def get_high(self): + return self._high + + def sample(self): + return self.sample_fn(self._shape) + + def __repr__(self): + return "[ContinuousActionSpace " + str(self._shape) + "]" + + @staticmethod + def _default_sample_fn(shape): + return np.random.normal(size=shape) + + +class DiscreteActionSpace(ActionSpace): + """Action space that takes integer inputs. + + Args: + shape (:obj:`list` of :obj:`int`): The shape of data that should be input to step or tick. + low (:obj:`int`): The lowest value to sample. + high (:obj:`int`): The highest value to sample. + buffer_shape (:obj:`list` of :obj:`int`, optional): The shape of the data that will be + written to the shared memory. + + Only use this when it is different from shape. + """ + + def __init__(self, shape, low, high, buffer_shape=None): + super(DiscreteActionSpace, self).__init__(shape, buffer_shape=buffer_shape) + self._low = low + self._high = high + + def sample(self): + return np.random.randint(self._low, self._high, self._shape, dtype=np.int32) + + def get_low(self): + return self._low + + def get_high(self): + return self._high + + def __repr__(self): + return "[DiscreteActionSpace " + str(self._shape) + ", min: " +\ + str(self._low) + ", max: " + str(self._high) + "]" diff --git a/PythonAPI/carla/source/holoocean/util.py b/PythonAPI/carla/source/holoocean/util.py new file mode 100644 index 0000000000..2eb1bd3928 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/util.py @@ -0,0 +1,95 @@ +"""Helpful Utilities""" +import math +import os +import holoocean +from multiprocessing import Process, Event + +try: + unicode # Python 2 +except NameError: + unicode = str # Python 3 + + +def get_holoocean_version(): + """Gets the current version of holoocean + + Returns: + (:obj:`str`): the current version + """ + return holoocean.__version__ + +def _get_holoocean_folder(): + if "HOLODECKPATH" in os.environ and os.environ["HOLODECKPATH"] != "": + return os.environ["HOLODECKPATH"] + + if os.name == "posix": + return os.path.expanduser("~/.local/share/holoocean") + + if os.name == "nt": + return os.path.expanduser("~\\AppData\\Local\\holoocean") + + raise NotImplementedError("holoocean is only supported for Linux and Windows") + +def get_holoocean_path(): + """Gets the path of the holoocean environment + + Returns: + (:obj:`str`): path to the current holoocean environment + """ + + return os.path.join(_get_holoocean_folder(), get_holoocean_version()) + + +def convert_unicode(value): + """Resolves python 2 issue with json loading in unicode instead of string + + Args: + value (:obj:`str`): Unicode value to be converted + + Returns: + (:obj:`str`): Converted string + + """ + if isinstance(value, dict): + return {convert_unicode(key): convert_unicode(value) + for key, value in value.iteritems()} + + if isinstance(value, list): + return [convert_unicode(item) for item in value] + + if isinstance(value, unicode): + return value.encode('utf-8') + + return value + + +def get_os_key(): + """Gets the key for the OS. + + Returns: + :obj:`str`: ``Linux`` or ``Windows``. Throws ``NotImplementedError`` for other systems. + """ + if os.name == "posix": + return "Linux" + if os.name == "nt": + return "Windows" + + raise NotImplementedError("HoloOcean is only supported for Linux and Windows") + + +def human_readable_size(size_bytes): + """Gets a number of bytes as a human readable string. + + Args: + size_bytes (:obj:`int`): The number of bytes to get as human readable. + + Returns: + :obj:`str`: The number of bytes in a human readable form. + """ + if size_bytes == 0: + return "0B" + size_name = ("B", "KB", "MB", "GB", "TB", "PB", "EB", "ZB", "YB") + base = int(math.floor(math.log(size_bytes, 1024))) + power = math.pow(1024, base) + size = round(size_bytes / power, 2) + return "%s %s" % (size, size_name[base]) diff --git a/PythonAPI/carla/source/holoocean/weather.py b/PythonAPI/carla/source/holoocean/weather.py new file mode 100644 index 0000000000..53d72722c4 --- /dev/null +++ b/PythonAPI/carla/source/holoocean/weather.py @@ -0,0 +1,110 @@ +"""Weather/time controller for environments""" +from holoocean.exceptions import HoloOceanException + + +class WeatherController: + """Controller for dynamically changing weather and time in an environment + + Args: + send_world_command (function): Callback for sending commands to a world + """ + def __init__(self, send_world_command): + self._send_command = send_world_command + self.cur_weather = "sunny" + + def set_fog_density(self, density): + """Change the fog density. + + The change will occur when :meth:`tick` or :meth:`step` is called next. + + By the next tick, the exponential height fog in the world will have the new density. If + there is no fog in the world, it will be created with the given density. + + Args: + density (:obj:`float`): The new density value, between 0 and 1. The command will not be + sent if the given density is invalid. + """ + if density < 0 or density > 1: + raise HoloOceanException("Fog density should be between 0 and 1") + + self._send_command("SetFogDensity", num_params=[density]) + + def set_day_time(self, hour): + """Change the time of day. + + Daytime will change when :meth:`tick` or :meth:`step` is called next. + + By the next tick, the lighting and the skysphere will be updated with the new hour. + + If there is no skysphere, skylight, or directional source light in the world, this command + will exit the environment. + + Args: + hour (:obj:`int`): The hour in 24-hour format: [0, 23]. + """ + self._send_command("SetHour", num_params=[hour % 24]) + + def start_day_cycle(self, day_length): + """Start the day cycle. + + The cycle will start when :meth:`tick` or :meth:`step` is called next. + + The sky sphere will then update each tick with an updated sun angle as it moves about the + sky. The length of a day will be roughly equivalent to the number of minutes given. + + If there is no skysphere, skylight, or directional source light in the world, this command + will exit the environment. + + Args: + day_length (:obj:`int`): The number of minutes each day will be. + """ + if day_length <= 0: + raise HoloOceanException("The given day length should be between above 0!") + + self._send_command("SetDayCycle", num_params=[1, day_length]) + + def stop_day_cycle(self): + """Stop the day cycle. + + The cycle will stop when :meth:`tick` or :meth:`step` is called next. + + By the next tick, day cycle will stop where it is. + + If there is no skysphere, skylight, or directional source light in the world, this command + will exit the environment. + """ + self._send_command("SetDayCycle", num_params=[0, -1]) + + def set_weather(self, weather_type): + """Set the world's weather. + + The new weather will be applied when :meth:`tick` or :meth:`step` is called next. + + By the next tick, the lighting, skysphere, fog, and relevant particle systems will be + updated and/or spawned + to the given weather. + + If there is no skysphere, skylight, or directional source light in the world, this command + will exit the environment. + + .. note:: + Because this command can affect the fog density, any changes made by a + ``change_fog_density`` command before a set_weather command called will be undone. It is + recommended to call ``change_fog_density`` after calling set weather if you wish to + apply your specific changes. + + In all downloadable worlds, the weather is sunny by default. + + If the given type string is not available, the command will not be sent. + + Args: + weather_type (:obj:`str`): The type of weather, which can be ``rain``, ``cloudy``, or + ``sunny``. + + """ + weather_type = weather_type.lower() + if not weather_type in ["rain", "cloudy", "sunny"]: + raise HoloOceanException("Invalid weather type " + weather_type) + + self.cur_weather = weather_type + self._send_command("SetWeather", string_params=[weather_type]) diff --git a/PythonAPI/examples/holoocean.py b/PythonAPI/examples/holoocean.py new file mode 100644 index 0000000000..7833690692 --- /dev/null +++ b/PythonAPI/examples/holoocean.py @@ -0,0 +1,143 @@ +"""This file contains multiple examples of how you might use HoloOcean.""" +import numpy as np + +import holoocean +from holoocean import agents +from holoocean.environments import * +from holoocean import sensors + +def hovering_example(): + """A basic example of how to use the HoveringAUV agent.""" + env = holoocean.make("SimpleUnderwater-Hovering") + + # This command tells the AUV go forward with a power of "10" + # The last four elements correspond to the horizontal thrusters (see docs for more info) + command = np.array([0, 0, 0, 0, 10, 10, 10, 10]) + for _ in range(1000): + state = env.step(command) + # To access specific sensor data: + if "PoseSensor" in state: + pose = state["PoseSensor"] + # Some sensors don't tick every timestep, so we check if it's received. + if "DVLSensor" in state: + dvl = state["DVLSensor"] + + # This command tells the AUV to go down with a power of "10" + # The first four elements correspond to the vertical thrusters + command = np.array([-10, -10, -10, -10, 0, 0, 0, 0]) + for _ in range(1000): + # We alternatively use the act function + env.act("auv0", command) + state = env.tick() + + # You can control the AgentFollower camera (what you see) by pressing v to toggle spectator + # mode. This detaches the camera and allows you to move freely about the world. + # Press h to view the agents x-y-z location + # You can also press c to snap to the location of the camera to see the world from the perspective of the + # agent. See the Controls section of the ReadMe for more details. + + +def torpedo_example(): + """A basic example of how to use the TorpedoAUV agent.""" + env = holoocean.make("SimpleUnderwater-Torpedo") + + # This command tells the AUV go forward with a power of "50" + # The last four elements correspond to + command = np.array([0, 0, 0, 0, 50]) + for _ in range(1000): + state = env.step(command) + + # Now turn the top and bottom fins to turn left + command = np.array([0, -45, 0, 45, 50]) + for _ in range(1000): + state = env.step(command) + + +def editor_example(): + """This editor example shows how to interact with holodeck worlds while they are being built + in the Unreal Engine Editor. Most people that use holodeck will not need this. + + This example uses a custom scenario, see + https://holoocean.readthedocs.io/en/latest/usage/examples/custom-scenarios.html + + Note: When launching Holodeck from the editor, press the down arrow next to "Play" and select + "Standalone Game", otherwise the editor will lock up when the client stops ticking it. + """ + + config = { + "name": "test", + "world": "ExampleLevel", + "main_agent": "auv0", + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "LocationSensor", + }, + { + "sensor_type": "VelocitySensor" + }, + { + "sensor_type": "RGBCamera" + } + ], + "control_scheme": 1, + "location": [0, 0, 1] + } + ] + } + + env = HoloOceanEnvironment(scenario=config, start_world=False) + command = [0, 0, 10, 50] + + for i in range(10): + env.reset() + for _ in range(1000): + state = env.step(command) + + +def editor_multi_agent_example(): + """This editor example shows how to interact with holodeck worlds that have multiple agents. + This is specifically for when working with UE4 directly and not a prebuilt binary. + + Note: When launching Holodeck from the editor, press the down arrow next to "Play" and select + "Standalone Game", otherwise the editor will lock up when the client stops ticking it. + """ + config = { + "name": "test_handagent", + "world": "ExampleLevel", + "main_agent": "auv0", + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + ], + "control_scheme": 1, + "location": [0, 0, 1] + }, + { + "agent_name": "auv1", + "agent_type": "TorpedoAUV", + "sensors": [ + ], + "control_scheme": 1, + "location": [0, 0, 5] + } + ] + } + + env = HoloOceanEnvironment(scenario=config, start_world=False) + + cmd0 = np.array([0, 0, -2, 10]) + cmd1 = np.array([0, 0, 5, 10]) + + for i in range(10): + env.reset() + env.act("uav0", cmd0) + env.act("uav1", cmd1) + for _ in range(1000): + states = env.tick() + diff --git a/PythonAPI/examples/holoocean/getting_started.py b/PythonAPI/examples/holoocean/getting_started.py new file mode 100644 index 0000000000..13805feb7b --- /dev/null +++ b/PythonAPI/examples/holoocean/getting_started.py @@ -0,0 +1,10 @@ +import holoocean +import numpy as np + +env = holoocean.make("PierHarbor-Hovering") + +# 悬停的AUV对每个推进器发出指令 +command = np.array([10,10,10,10,0,0,0,0]) + +for _ in range(1800): + state = env.step(command) \ No newline at end of file diff --git a/PythonAPI/examples/holoocean/installation.py b/PythonAPI/examples/holoocean/installation.py new file mode 100644 index 0000000000..047f594d1a --- /dev/null +++ b/PythonAPI/examples/holoocean/installation.py @@ -0,0 +1,4 @@ +import holoocean + +# 需要等待一段时间，从 https://robots.et.byu.edu/holo/Ocean/v1.0.0/Windows.zip 安装 Ocean 到目录 C:\Users\nongf\AppData\Local\holoocean\1.0.0\worlds\Ocean +holoocean.install("Ocean") \ No newline at end of file diff --git a/PythonAPI/requirements_hutb.txt b/PythonAPI/requirements_hutb.txt index 60e05da0bb..d375f94aad 100644 --- a/PythonAPI/requirements_hutb.txt +++ b/PythonAPI/requirements_hutb.txt @@ -9,4 +9,4 @@ backports.weakref backports.ssl_match_hostname GitPython pyinstaller -./carla/dist/hutb-2.10.0-cp310-cp310-win_amd64.whl +./carla/dist/hutb-2.10.1-cp310-cp310-win_amd64.whl diff --git a/PythonAPI/test/holoocean/README.md b/PythonAPI/test/holoocean/README.md new file mode 100644 index 0000000000..998e797a44 --- /dev/null +++ b/PythonAPI/test/holoocean/README.md @@ -0,0 +1,52 @@ +# HoloOcean Integration Tests + +## Pre-reqs + +`pip install tox` + +### What is tox +Tox automatically creates a virtualenv with the dependencies specified in +`config.py` and runs `pytest` against that virtualenv. This allows us to +test holoocean like it were a package installed on a fresh machine. + +## I just want to run tests against the code I'm working with + +### Install your development copy of holoocean +You can have pip install the `holoocean` module in "editable" mode - meaning +you can `import holoocean` anywhere, and it will use the code in +`src/holoocean`. + +First, remove whatever version of holoocean you have installed +`pip uninstall holoocean` + +Then, from the root of this repo run +`pip install --editable .` + +Open a `python` terminal and make sure when you `import holoocean` and run +`holoocean.util.get_holoocean_version()` it prints `"X.Y.Zdev"` + +### Run pytest + +Once you have installed your dev copy of holoocean, run +`pytest` from the root of this repo. You should see output like this +``` +============================= test session starts ============================= +platform win32 -- Python 3.7.1, pytest-4.5.0, py-1.8.0, pluggy-0.12.0 +cachedir: .tox\py37\.pytest_cache +rootdir: C:\Users\jayde\Documents\holoocean, inifile: pytest.ini +collected 28 items + +tests\scenarios\test_loading_scenarios.py .............. [ 50%] +tests\scenarios\test_reset.py ....... [ 75%] +tests\scenarios\test_rgb_camera_not_null.py ....... [100%] + +========================= 28 passed in 131.77 seconds ========================= +___________________________________ summary ___________________________________ + py37: commands succeeded + congratulations :) +``` + +In Pycharm, you can also right click on a test and run/debug it individually + +## Run Tox +Just type `tox` from the root of this repo. \ No newline at end of file diff --git a/PythonAPI/test/holoocean/__init__.py b/PythonAPI/test/holoocean/__init__.py new file mode 100644 index 0000000000..3df74a58df --- /dev/null +++ b/PythonAPI/test/holoocean/__init__.py @@ -0,0 +1 @@ +# This file is needed so the utils module can be imported correctly diff --git a/PythonAPI/test/holoocean/agents/__init__.py b/PythonAPI/test/holoocean/agents/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/PythonAPI/test/holoocean/agents/test_getting_action_space_constraints.py b/PythonAPI/test/holoocean/agents/test_getting_action_space_constraints.py new file mode 100644 index 0000000000..1ac8dc45fa --- /dev/null +++ b/PythonAPI/test/holoocean/agents/test_getting_action_space_constraints.py @@ -0,0 +1,95 @@ +import uuid +from holoocean import packagemanager as pm +from holoocean.environments import HoloOceanEnvironment +import math + +test_data = [ + # agent_name, control_scheme, exp_min, exp_max + { + "type": "UavAgent", + "name": "UavAgent", + "control_scheme": 0, + "min": [-5.087, -6.5080, -0.8, -59.844], + "max": [5.087, 6.5080, 0.8, 59.844], + }, + { + "type": "SphereAgent", + "name": "SphereAgent0", + "control_scheme": 0, + "min": 0, + "max": 4, + }, + { + "type": "SphereAgent", + "name": "SphereAgent1", + "control_scheme": 1, + "min": [-20, -20], + "max": [20, 20], + }, + { + "type": "TurtleAgent", + "name": "TurtleAgent", + "control_scheme": 0, + "min": [-160.0, -35.0], + "max": [160.0, 35.0], + }, +] + +agents = [ + { + "agent_name": x["name"], + "agent_type": x["type"], + "sensors": [], + "control_scheme": x["control_scheme"], + "location": [0, 0, 5], + } + for x in test_data +] + +config = { + "name": "test_collision_sensor", + "world": "TestWorld", + "main_agent": "UavAgent", + "frames_per_sec": False, + "agents": agents, +} + + +def is_close(a, b): + for x, y in zip(a, b): + if not math.isclose(x, y, rel_tol=1e-05): + return False + + return True + + +def check_constraints(x, y): + if type(x) == list: + assert is_close(x, y) + else: + assert math.isclose(x, y, rel_tol=1e-05) + + +def test_min_max_action_space_constraints(): + binary_path = pm.get_binary_path_for_package("TestWorlds") + + with HoloOceanEnvironment( + scenario=config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for x in test_data: + agent = x["name"] + control_scheme = x["control_scheme"] + + min_result = env.agents[agent].control_schemes[control_scheme][1].get_low() + exp_min = x["min"] + + check_constraints(min_result, exp_min) + + max_result = env.agents[agent].control_schemes[control_scheme][1].get_high() + exp_max = x["max"] + + check_constraints(max_result, exp_max) diff --git a/PythonAPI/test/holoocean/agents/test_hovering.py b/PythonAPI/test/holoocean/agents/test_hovering.py new file mode 100644 index 0000000000..bf08e1be3c --- /dev/null +++ b/PythonAPI/test/holoocean/agents/test_hovering.py @@ -0,0 +1,77 @@ +import holoocean +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_hovering", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "DynamicsSensor", + } + ], + "control_scheme": 0, + "location": [0, 0, -10], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_pid_controller(env): + """Test to make sure it goes to the orientation and position we command""" + des = [2, 2, -12, 0, 20, 45] + env._scenario["agents"][0]["control_scheme"] = 1 + + env.reset() + + state = env.step(des, 200) + + pos = state["DynamicsSensor"][6:9] + rpy = state["DynamicsSensor"][15:18] + + assert np.allclose( + des[:3], pos, 1e-1 + ), "Controller didn't make it to the right position" + assert np.allclose( + des[3:], rpy, 5 + ), "Controller didn't make it to the right orientation" + + +def test_manual_dynamics(env): + """Test to make sure it goes to the linear and angular acceleration we set""" + des = [1, 2, 3, 0.1, 0.2, 0.3] + env._scenario["agents"][0]["control_scheme"] = 2 + + env.reset() + + state = env.step(des, 20) + + accel = state["DynamicsSensor"][0:3] + ang_accel = state["DynamicsSensor"][9:12] + + assert np.allclose( + des[:3], accel + ), "Manual dynamics didn't hit the correct linear acceleration" + assert np.allclose( + des[3:], ang_accel + ), "Manual dynamics didn't hit the correct angular acceleration" diff --git a/PythonAPI/test/holoocean/agents/test_randomization.py b/PythonAPI/test/holoocean/agents/test_randomization.py new file mode 100644 index 0000000000..af03099c22 --- /dev/null +++ b/PythonAPI/test/holoocean/agents/test_randomization.py @@ -0,0 +1,107 @@ +import uuid +import copy +import numpy as np +import math +from holoocean import packagemanager as pm +from holoocean.environments import HoloOceanEnvironment + +base_conf = { + "name": "test_randomization", + "world": "TestWorld", + "package_name": "TestWorlds", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + {"sensor_type": "LocationSensor"}, + {"sensor_type": "RotationSensor"}, + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + "rotation": [1.0, 2.0, 3.0], + "location_randomization": [40, 40, 40], + "rotation_randomization": [40, 40, 40], + } + ], +} + + +def is_different_3d_vector(lhs, rhs): + is_same = np.isclose(lhs, rhs, rtol=1e-12) + return not is_same[0] or not is_same[1] or not is_same[2] + + +def check_3d_vector_variance(lhs, rhs, variance): + first = math.isclose(lhs[0], rhs[0], rel_tol=variance[0]) + second = math.isclose(lhs[1], rhs[1], rel_tol=variance[1]) + third = math.isclose(lhs[1], rhs[1], rel_tol=variance[1]) + + return first and second and third + + +def test_location_with_randomization(): + """ + Validate that the location of the agent is not the same between resets + Args: + + Returns: + + """ + bin_path = pm.get_binary_path_for_package("TestWorlds") + conf = copy.deepcopy(base_conf) + + with HoloOceanEnvironment( + scenario=conf, + verbose=True, + binary_path=bin_path, + show_viewport=False, + uuid=str(uuid.uuid4()), + ) as env: + prev_location = conf["agents"][0]["location"] + default_start_location = conf["agents"][0]["location"] + variance = conf["agents"][0]["location_randomization"] + + num_resets = 5 + for _ in range(num_resets): + cur_location = env.tick()["LocationSensor"] + + assert is_different_3d_vector(cur_location, prev_location) + assert check_3d_vector_variance( + cur_location, default_start_location, variance + ) + + prev_location = cur_location + env.reset() + + +def test_rotation_with_randomization(): + """ + Validate that the rotation of the agent is not the same between resets + Args: + + Returns: + + """ + bin_path = pm.get_binary_path_for_package("TestWorlds") + conf = copy.deepcopy(base_conf) + + with HoloOceanEnvironment( + scenario=conf, + binary_path=bin_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + prev_start_rotation = conf["agents"][0]["rotation"] + + num_resets = 5 + for _ in range(num_resets): + cur_rotation = env.tick()["RotationSensor"] + + assert is_different_3d_vector(cur_rotation, prev_start_rotation) + + prev_start_rotation = cur_rotation + env.reset() diff --git a/PythonAPI/test/holoocean/agents/test_surface_vessel.py b/PythonAPI/test/holoocean/agents/test_surface_vessel.py new file mode 100644 index 0000000000..65b022830f --- /dev/null +++ b/PythonAPI/test/holoocean/agents/test_surface_vessel.py @@ -0,0 +1,99 @@ +import holoocean +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_hovering", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "SurfaceVessel", + "sensors": [ + { + "sensor_type": "DynamicsSensor", + } + ], + "control_scheme": 0, + "location": [10, 10, 3], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_buoyancy_from_sunk(env): + """Make sure it floats on the surface, from initial sunk position""" + # Going up + env._scenario["agents"][0]["control_scheme"] = 0 + env._scenario["agents"][0]["location"] = [10, 10, -5] + + env.reset() + state = env.tick(100) + p = state["DynamicsSensor"][6:9] + + assert p[2] > -1, "Surface Vessel didn't float" + assert p[2] < 1, "Surface Vessel didn't stop going up" + + +def test_buoyancy_from_drop(env): + """Make sure it floats on the surface, from initial drop.""" + # Going down + env._scenario["agents"][0]["control_scheme"] = 0 + env._scenario["agents"][0]["location"] = [10, 10, 5] + + env.reset() + state = env.tick(100) + p = state["DynamicsSensor"][6:9] + + assert p[2] > -1, "Surface Vessel didn't float" + assert p[2] < 1, "Surface Vessel didn't drop" + + +def test_pid_controller(env): + """Test to make sure it goes to the orientation and position we command""" + des = [20, 20] + env._scenario["agents"][0]["control_scheme"] = 1 + + env.reset() + + state = env.step(des, 300) + + pos = state["DynamicsSensor"][6:8] + + assert np.allclose(des, pos, 1), "Controller didn't make it to the right position" + + +def test_manual_dynamics(env): + """Test to make sure it goes to the linear and angular acceleration we set""" + des = [1, 2, 3, 0.1, 0.2, 0.3] + env._scenario["agents"][0]["control_scheme"] = 2 + + env.reset() + + state = env.step(des, 20) + + accel = state["DynamicsSensor"][0:3] + ang_accel = state["DynamicsSensor"][9:12] + + assert np.allclose( + des[:3], accel + ), "Manual dynamics didn't hit the correct linear acceleration" + assert np.allclose( + des[3:], ang_accel + ), "Manual dynamics didn't hit the correct angular acceleration" diff --git a/PythonAPI/test/holoocean/agents/test_torpedo.py b/PythonAPI/test/holoocean/agents/test_torpedo.py new file mode 100644 index 0000000000..07c375062e --- /dev/null +++ b/PythonAPI/test/holoocean/agents/test_torpedo.py @@ -0,0 +1,56 @@ +import holoocean +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_torpedo", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "TorpedoAUV", + "sensors": [ + { + "sensor_type": "DynamicsSensor", + } + ], + "control_scheme": 1, + "location": [0, 0, -10], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_manual_dynamics(env): + """Test to make sure it goes to the linear and angular acceleration we set""" + des = [1, 2, 3, 0.1, 0.2, 0.3] + + env.reset() + + state = env.step(des, 20) + + accel = state["DynamicsSensor"][0:3] + ang_accel = state["DynamicsSensor"][9:12] + + assert np.allclose( + des[:3], accel + ), "Manual dynamics didn't hit the correct linear acceleration" + assert np.allclose( + des[3:], ang_accel + ), "Manual dynamics didn't hit the correct angular acceleration" diff --git a/PythonAPI/test/holoocean/generate_test_images.py b/PythonAPI/test/holoocean/generate_test_images.py new file mode 100644 index 0000000000..9a315b4a62 --- /dev/null +++ b/PythonAPI/test/holoocean/generate_test_images.py @@ -0,0 +1,108 @@ +import holoocean +import cv2 +import copy +import os +import uuid + +# from tests.utils.equality import mean_square_err + +base_cfg = { + "name": "test_viewport_capture", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [{"sensor_type": "ViewportCapture"}], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +def test_viewport_capture(resolution): + """Validates that the ViewportCapture camera is working at the expected resolutions + + Also incidentally validates that the viewport can be sized correctly + """ + + global base_cfg + + cfg = copy.deepcopy(base_cfg) + + cfg["window_width"] = resolution + cfg["window_height"] = resolution + + cfg["agents"][0]["sensors"][0]["configuration"] = { + "CaptureWidth": resolution, + "CaptureHeight": resolution, + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + env.should_render_viewport(True) + env.tick(5) + env.should_render_viewport(True) + env.tick(5) + + pixels = env.tick()["ViewportCapture"][:, :, 0:3] + filename = str("baseline_viewport_" + str(resolution) + ".png") + + cv2.imwrite(filename, pixels) + cv2.waitKey(0) + + +def test_rgb_camera(resolution): + """Makes sure that the RGB camera is positioned and capturing correctly. + + Capture pixel data, and load from disk the baseline of what it should look like. + Then, use mse() to see how different the images are. + + """ + + global base_cfg + + cfg = copy.deepcopy(base_cfg) + + cfg["agents"][0]["sensors"][0]["sensor_type"] = "RGBCamera" + cfg["agents"][0]["sensors"][0]["socket"] = "CameraSocket" + cfg["agents"][0]["sensors"][0]["configuration"] = { + "CaptureWidth": resolution, + "CaptureHeight": resolution, + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(5): + env.tick() + + pixels = env.tick()["TestCamera"][:, :, 0:3] + filepath = str("baseline_temp_" + str(resolution) + ".png") + cv2.imwrite(filepath, pixels) + cv2.imshow("image", pixels) + cv2.waitKey(0) + + +if __name__ == "__main__": + print("Hello") + # test_viewport_capture(256) + test_viewport_capture(500) + test_viewport_capture(1000) + + # test_sensor_rotation(rotation_env()) diff --git a/PythonAPI/test/holoocean/lcm/test_python_python.py b/PythonAPI/test/holoocean/lcm/test_python_python.py new file mode 100644 index 0000000000..a96c7503d0 --- /dev/null +++ b/PythonAPI/test/holoocean/lcm/test_python_python.py @@ -0,0 +1,84 @@ +import holoocean +import uuid +import pytest + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test", + "world": "TestWorld", + "main_agent": "turtle0", + "lcm_provider": "memq://", + "frames_per_sec": False, + "octree_min": 0.1, + "octree_max": 10, + "env_min": [-1, -1, -1], + "env_max": [1, 1, 1], + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "IMUSensor", + "publish": "lcm", + "lcm_channel": "sensor", + "configuration": { + "ReturnBias": True, # for IMU + "ReturnRange": True, # for DVL + }, + } + ], + "control_scheme": 0, + "location": [-1.5, 1.50, 3.0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +@pytest.mark.parametrize( + "sensor", + [ + "DVLSensor", + "IMUSensor", + "GPSSensor", + "ImagingSonar", + "DepthSensor", + "RGBCamera", + "PoseSensor", + "LocationSensor", + "RangeFinderSensor", + "RotationSensor", + "OrientationSensor", + "VelocitySensor", + ], +) +def test_sensor(env, sensor): + env._scenario["agents"][0]["sensors"][0]["sensor_type"] = sensor + env.reset() + + d = {"i": 0} + + def my_handler(channel, data): + d["i"] += 1 + + sub = env._lcm.subscribe("sensor", my_handler) + + for _ in range(50): + env.tick() + env._lcm.handle() + + assert d["i"] == 50, f"LCM only received {d['i']} of 100 messages" + + env._lcm.unsubscribe(sub) diff --git a/PythonAPI/test/holoocean/lcm/test_python_python_acoustic_beacon.py b/PythonAPI/test/holoocean/lcm/test_python_python_acoustic_beacon.py new file mode 100644 index 0000000000..e4bcb0c3ae --- /dev/null +++ b/PythonAPI/test/holoocean/lcm/test_python_python_acoustic_beacon.py @@ -0,0 +1,143 @@ +import holoocean +from holoocean.lcm import AcousticBeaconSensor +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test", + "world": "TestWorld", + "main_agent": "turtle0", + "lcm_provider": "memq://", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "sensor_name": "Zero", + "location": [0, 0, 0], + "configuration": {"id": 0}, + }, + { + "sensor_type": "AcousticBeaconSensor", + "sensor_name": "One", + "location": [1, 0, 0], + "configuration": {"id": 1}, + "lcm_channel": "sensor", + }, + ], + "control_scheme": 0, + "location": [-1.5, 1.50, 3.0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_acoustic_beacon(env): + d = {"i": 0} + + def my_handler(channel, data): + d["i"] += 1 + + env.reset() + + sub = env._lcm.subscribe("sensor", my_handler) + + for _ in range(100): + env.send_acoustic_message(0, 1, "OWAY", None) + env.tick() + env._lcm.handle() + + assert d["i"] == 100, f"LCM only received {d['i']} of 100 messages" + + env._lcm.unsubscribe(sub) + + +@pytest.mark.parametrize( + "msg", + [ + "OWAY", + "OWAYU", + "MSG_REQU", + "MSG_RESPU", + "MSG_REQ", + "MSG_RESP", + "MSG_REQX", + "MSG_RESPX", + ], +) +def test_acoustic_types(msg, env): + def my_handler(channel, data): + temp = AcousticBeaconSensor.decode(data) + + assert temp.from_beacon == 0 + if temp.msg_type == "OWAY": + assert np.isnan(temp.azimuth) + assert np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "OWAYU": + assert ~np.isnan(temp.azimuth) + assert ~np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "MSG_REQ": + assert np.isnan(temp.azimuth) + assert np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "MSG_RESP": + assert np.isnan(temp.azimuth) + assert np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "MSG_REQU": + assert ~np.isnan(temp.azimuth) + assert ~np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "MSG_RESPU": + assert ~np.isnan(temp.azimuth) + assert ~np.isnan(temp.elevation) + assert ~np.isnan(temp.range) + assert np.isnan(temp.z) + elif temp.msg_type == "MSG_REQX": + assert ~np.isnan(temp.azimuth) + assert ~np.isnan(temp.elevation) + assert np.isnan(temp.range) + assert ~np.isnan(temp.z) + elif temp.msg_type == "MSG_RESPX": + assert ~np.isnan(temp.azimuth) + assert ~np.isnan(temp.elevation) + assert ~np.isnan(temp.range) + assert ~np.isnan(temp.z) + + env.reset() + + sub = env._lcm.subscribe("sensor", my_handler) + + # Only send every other tick so we don't interfere with returning messages + for i in range(100): + if i % 2 == 0: + env.send_acoustic_message(0, 1, msg, None) + env.tick() + if i % 2 == 1: + env._lcm.handle() + + env._lcm.unsubscribe(sub) diff --git a/PythonAPI/test/holoocean/pytest.ini b/PythonAPI/test/holoocean/pytest.ini new file mode 100644 index 0000000000..98c27dc9c2 --- /dev/null +++ b/PythonAPI/test/holoocean/pytest.ini @@ -0,0 +1,3 @@ +[pytest] +filterwarnings = + ignore:.*Assigning the .*inherently unsafe.*:DeprecationWarning \ No newline at end of file diff --git a/PythonAPI/test/holoocean/scenarios/__init__.py b/PythonAPI/test/holoocean/scenarios/__init__.py new file mode 100644 index 0000000000..e5e7913ba8 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/__init__.py @@ -0,0 +1,7 @@ +""" +Validation for scenarios. + +Within this module, if you parameterize with `env_scenario`, pytest will cache the +instances of holoocean between test runs to speed things up. If you want to instead +create the environment yourself, use `scenario` +""" diff --git a/PythonAPI/test/holoocean/scenarios/conftest.py b/PythonAPI/test/holoocean/scenarios/conftest.py new file mode 100644 index 0000000000..0bc4bd3120 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/conftest.py @@ -0,0 +1,28 @@ +from typing import Callable, List + +import holoocean +import pytest +from holoocean import packagemanager as pm +from holoocean.environments import HoloOceanEnvironment +from holoocean.sensors import SensorDefinition + + +def pytest_generate_tests(metafunc): + """Iterate over every scenario""" + scenarios = set() + for config, full_path in pm._iter_packages(): + for world_entry in config["worlds"]: + for config, full_path in pm._iter_scenarios(world_entry["name"]): + # Don't make ones with a sonar + use = True + name = "{}-{}".format(config["world"], config["name"]) + config = holoocean.packagemanager.get_scenario(name) + for agent in config["agents"]: + for sensor in agent["sensors"]: + if sensor["sensor_type"] in SensorDefinition._sonar_sensors: + use = False + if use: + scenarios.add(name) + + if "scenario" in metafunc.fixturenames: + metafunc.parametrize("scenario", scenarios) diff --git a/PythonAPI/test/holoocean/scenarios/test_loading_scenarios.py b/PythonAPI/test/holoocean/scenarios/test_loading_scenarios.py new file mode 100644 index 0000000000..0bef5c8828 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_loading_scenarios.py @@ -0,0 +1,45 @@ +import holoocean + + +def test_load_scenario(scenario): + """Tests that every scenario can be loaded without any errors + + Also test that every agent has every sensor that is present in the config file + + TODO: We need some way of communicating with the engine to verify that the expected level was loaded. + If the level isn't found, then Unreal just picks a default one, so we're missing that case + + Args: + scenario (str): Scenario to test + + """ + env = holoocean.make( + scenario, show_viewport=False, frames_per_sec=False, verbose=True + ) + scenario = holoocean.packagemanager.get_scenario(scenario) + + # make sure it works! + for _ in range(20): + env.tick() + + # make sure everything is there + assert len(env.agents) == len(scenario["agents"]), "Length of agents did not match!" + + for agent in scenario["agents"]: + assert agent["agent_name"] in env.agents, "Agent is not in the environment!" + + assert len(agent["sensors"]) == len( + env.agents[agent["agent_name"]].sensors + ), "length of sensors did not match!" + + for sensor in agent["sensors"]: + sensor_name = ( + sensor["sensor_name"] + if "sensor_name" in sensor + else sensor["sensor_type"] + ) + assert ( + sensor_name in env.agents[agent["agent_name"]].sensors + ), "Sensor is missing!" + + env.__on_exit__() diff --git a/PythonAPI/test/holoocean/scenarios/test_prop_spawning.py b/PythonAPI/test/holoocean/scenarios/test_prop_spawning.py new file mode 100644 index 0000000000..ca65c0940f --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_prop_spawning.py @@ -0,0 +1,84 @@ +import holoocean +import uuid + +from tests.utils.equality import almost_equal + +uav_config = { + "name": "test_prop_spawning", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "LocationSensor", + } + ], + "control_scheme": 0, + "location": [0, 0, 5], + } + ], +} + + +def test_static_prop(): + """Tests whether spawning a box without sim_physics creates a static box that keeps + the agent from falling. + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=uav_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # spawn a platform for the uav to rest on + env.spawn_prop("box", location=[0, 0, 4.5], scale=[5, 5, 0.5]) + + # get the initial location after the uav has settled + init_location = env.tick(10)["LocationSensor"] + + # get the final location + final_location = env.tick(50)["LocationSensor"] + + assert almost_equal( + init_location, final_location + ), "Uav \ + continued to fall despite spawning a platform underneath!" + + +def test_sim_physics_prop(): + """Tests whether spawning a sphere with simulated physics creates a sphere that falls + and rams the agent. + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=uav_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + env.agents["uav0"].teleport([0, 0, 0.1]) + + # get the initial location after the uav has settled + init_location = env.tick(20)["LocationSensor"] + + # spawn a ball to fall on top of the agent + env.spawn_prop("sphere", location=[0, 0.1, 3], scale=3, sim_physics=True) + + # get the final location after the uav has been knocked over + final_location = env.tick(100)["LocationSensor"] + + assert not almost_equal( + init_location, final_location + ), "Uav \ + wasn't hit by a spawned sphere!" diff --git a/PythonAPI/test/holoocean/scenarios/test_reset.py b/PythonAPI/test/holoocean/scenarios/test_reset.py new file mode 100644 index 0000000000..7a5ce22c72 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_reset.py @@ -0,0 +1,62 @@ +import numpy as np + +import holoocean + + +def compare_agent_states(state1, state2, thresh=0.01, is_close=True, to_ignore=None): + if to_ignore is None: + to_ignore = [] + + for sensor in state1: + if sensor in to_ignore: + continue + close = almost_equal(state1[sensor], state2[sensor], thresh) + if is_close != close: + print("Sensor {} failed!".format(sensor)) + print(state1[sensor]) + print(state2[sensor]) + print("--------------------------------") + assert is_close != close + + +def almost_equal(item1, item2, r_thresh=0.01, a_thresh=1e-4): + item1 = np.array(item1).flatten() + item2 = np.array(item2).flatten() + if len(item1) != len(item2): + return False + return all(np.isclose(item1, item2, rtol=r_thresh, atol=a_thresh)) + + +def is_full_state(state): + return isinstance(next(iter(state.values())), dict) + + +# def test_main_agent_after_resetting(env_scenario): +# """Validate that sensor data for the main agent is the same after calling .reset() + +# Args: +# env_scenario ((HoloOceanEnvironment, str)): environment and scenario we are testing + +# """ + + +# env, scenario = env_scenario +# scenario_config = holoocean.packagemanager.get_scenario(scenario) + +# main_agent = scenario_config["main_agent"] + +# test_resets = 5 + +# env.reset() +# init_state = env._get_full_state()[main_agent] +# agent_count = len(env.agents) +# sensor_count = sum([len(env.agents[agent].sensors) for agent in env.agents]) + +# for _ in range(test_resets): +# env.tick() +# env.reset() +# state = env._get_full_state()[main_agent] + +# compare_agent_states(init_state, state, 0.3, is_close=True, to_ignore=["RGBCamera", "BallLocationSensor"]) +# assert agent_count == len(env.agents) +# assert sensor_count == sum([len(env.agents[agent].sensors) for agent in env.agents]) diff --git a/PythonAPI/test/holoocean/scenarios/test_rgb_camera_not_null.py b/PythonAPI/test/holoocean/scenarios/test_rgb_camera_not_null.py new file mode 100644 index 0000000000..7457ed7f83 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_rgb_camera_not_null.py @@ -0,0 +1,36 @@ +# import holoocean + + +# def test_rgb_camera_not_null(env_scenario): +# """Test that the RGBCamera is sending sensor data by ensuring that it is not all zeros + +# Args: +# env_scenario ((HoloOceanEnvironment, str)): environment and scenario we are testing + +# """ +# env, scenario = env_scenario + +# # Find the names of every RGB camera and agent in the scenario +# config = holoocean.packagemanager.get_scenario(scenario) + +# # Set of tuples of agent name to camera name +# agent_camera_names = set() +# for agent_cfg in config["agents"]: +# for sensor_cfg in agent_cfg["sensors"]: +# if sensor_cfg["sensor_type"] == holoocean.sensors.RGBCamera.sensor_type: +# if "sensor_name" in sensor_cfg: +# sensor_name = sensor_cfg["sensor_name"] +# else: +# sensor_name = sensor_cfg["sensor_type"] +# agent_camera_names.add((agent_cfg["agent_name"], sensor_name)) + +# # Get pixel data +# state = env.tick() +# for agent, camera in agent_camera_names: +# if len(env.agents) == 1: +# pixels = state[camera] +# else: +# pixels = state[agent][camera] + +# # ensure that the pixels aren't all close to zero +# assert pixels.mean() > 1 diff --git a/PythonAPI/test/holoocean/scenarios/test_rotations.py b/PythonAPI/test/holoocean/scenarios/test_rotations.py new file mode 100644 index 0000000000..3eaebb2d11 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_rotations.py @@ -0,0 +1,93 @@ +import holoocean +import uuid +import pytest +import numpy as np +from scipy.spatial.transform import Rotation +from scipy.linalg import logm + + +def rot_error(A, B): + e = logm(A @ B.T) + return np.array([e[0, 1], e[0, 2], e[1, 2]]) + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "sphere", + "agents": [ + { + "agent_name": "sphere", + "agent_type": "SphereAgent", + "sensors": [ + {"sensor_type": "OrientationSensor", "rotation": [0, 0, 0]} + ], + "control_scheme": 0, + "location": [0, 0, 0], + "rotation": [0, 0, 0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(3)) +def test_actor_rotation(env, num): + """Make sure the orientation we tell the actor to start at is the correct one""" + angles = np.random.rand(3) * 20 + R = Rotation.from_euler("xyz", angles, degrees=True).as_matrix() + + env._scenario["agents"][0]["rotation"] = angles.tolist() + env._scenario["agents"][0]["sensors"][0]["rotation"] = [0, 0, 0] + + env.reset() + + state = env.tick() + + assert np.allclose(np.zeros(3), rot_error(R, state["OrientationSensor"]), atol=1e-5) + + +@pytest.mark.parametrize("num", range(3)) +def test_sensor_rotation(env, num): + """Make sure the orientation we tell the sensor to start at is the correct one""" + angles = np.random.rand(3) * 20 + + R = Rotation.from_euler("xyz", angles, degrees=True).as_matrix() + + env._scenario["agents"][0]["rotation"] = [0, 0, 0] + env._scenario["agents"][0]["sensors"][0]["rotation"] = angles.tolist() + + env.reset() + + state = env.tick() + + assert np.allclose(np.zeros(3), rot_error(R, state["OrientationSensor"]), atol=1e-5) + + +@pytest.mark.parametrize("num", range(3)) +def test_teleport_rotation(env, num): + """Make sure the orientation we teleport the agent to is the correct one""" + angles = np.random.rand(3) * 20 + + R = Rotation.from_euler("xyz", angles, degrees=True).as_matrix() + + env._scenario["agents"][0]["rotation"] = [0, 0, 0] + env._scenario["agents"][0]["sensors"][0]["rotation"] = [0, 0, 0] + + env.reset() + + state = env.tick() + env.agents["sphere"].teleport([0, 0, 0], angles.tolist()) + state = env.tick() + + assert np.allclose(np.zeros(3), rot_error(R, state["OrientationSensor"]), atol=1e-5) diff --git a/PythonAPI/test/holoocean/scenarios/test_using_make_with_custom_config.py b/PythonAPI/test/holoocean/scenarios/test_using_make_with_custom_config.py new file mode 100644 index 0000000000..100984898c --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_using_make_with_custom_config.py @@ -0,0 +1,38 @@ +import holoocean +import uuid + + +def test_using_make_with_custom_config(): + """ + Validate that we can use holoocean.make with a custom configuration instead + of loading it from a config file + """ + + sphere_config = { + "name": "test_custom_config", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=sphere_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(0, 10): + env.tick() + assert True diff --git a/PythonAPI/test/holoocean/scenarios/test_weather_scenarios.py b/PythonAPI/test/holoocean/scenarios/test_weather_scenarios.py new file mode 100644 index 0000000000..fa13216d91 --- /dev/null +++ b/PythonAPI/test/holoocean/scenarios/test_weather_scenarios.py @@ -0,0 +1,24 @@ +# from holoocean.environments import HoloOceanEnvironment + +# def test_weather_type_scenario(env_scenario) -> None: +# """Test that each weather type can be set in each scenario without errors. +# If a world doesn't have the WeatherController, the engine will crash and the +# test will fail. + +# This test only really needs to be done for each world, but since we already +# load up every scenario, it is faster to use those already initialized +# environments. + +# Args: +# env_scenario (HoloOceanEnvironment, str): Tuple to test + +# """ +# env, _ = env_scenario + +# weather_types = ["sunny", "cloudy", "rain"] + +# for weather in weather_types: +# env.weather.set_weather(weather) +# for _ in range(10): +# env.tick() +# env.reset() diff --git a/PythonAPI/test/holoocean/sensors/__init__.py b/PythonAPI/test/holoocean/sensors/__init__.py new file mode 100644 index 0000000000..a962c6fe39 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/__init__.py @@ -0,0 +1,6 @@ +""" +Tests in this module are intended to be for specific sensors, not for every world in general. +Ideally, there would be tests for every sensor available in holoocean, using the ExampleLevel or some +other world. + +""" diff --git a/PythonAPI/test/holoocean/sensors/baseline_images/README.md b/PythonAPI/test/holoocean/sensors/baseline_images/README.md new file mode 100644 index 0000000000..dd775f746e --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/baseline_images/README.md @@ -0,0 +1,3 @@ +# expected iamges + +https://github.com/BYU-PCCL/holodeck/tree/develop/tests/sensors/expected \ No newline at end of file diff --git a/PythonAPI/test/holoocean/sensors/conftest.py b/PythonAPI/test/holoocean/sensors/conftest.py new file mode 100644 index 0000000000..f4116159c3 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/conftest.py @@ -0,0 +1,167 @@ +import pytest + +import uuid +import holoocean + + +def pytest_generate_tests(metafunc): + """Iterate over every scenario""" + if "resolution" in metafunc.fixturenames: + metafunc.parametrize("resolution", [256, 500, 1000]) + elif "1024_env" in metafunc.fixturenames: + metafunc.parametrize("env_1024", [1024], indirect=True) + elif "ticks_per_capture" in metafunc.fixturenames: + metafunc.parametrize("ticks_per_capture", [30, 15, 10, 5, 2]) + elif "abuse_world" in metafunc.fixturenames: + metafunc.parametrize("abuse_world", ["abuse_world"], indirect=True) + elif "rotation_env" in metafunc.fixturenames: + metafunc.parametrize("rotation_env", ["rotation_env"], indirect=True) + elif "agent_abuse_world" in metafunc.fixturenames: + metafunc.parametrize( + "agent_abuse_world", ["turtle0", "uav0"], indirect=True + ) + + +shared_1024_env = None + + +@pytest.fixture +def env_1024(request): + """Shares the 1024x1024 configuration for use in two tests""" + cfg = { + "name": "test_viewport_capture", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "ViewportCapture", + "configuration": {"CaptureWidth": 1024, "CaptureHeight": 1024}, + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + "window_width": 1024, + "window_height": 1024, + } + + global shared_1024_env + + if shared_1024_env is None: + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + shared_1024_env = holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) + + shared_1024_env.reset() + + return shared_1024_env + + +shared_rotation_env = None +shared_abuse_env = None + + +def get_abuse_world(): + global shared_abuse_env + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + if shared_abuse_env is None: + shared_abuse_env = holoocean.environments.HoloOceanEnvironment( + scenario=abuse_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) + shared_abuse_env.reset() + return shared_abuse_env + + +@pytest.fixture +def abuse_world(request): + return get_abuse_world() + + +@pytest.fixture +def agent_abuse_world(request): + env = get_abuse_world() + return request.param, env + + +@pytest.fixture +def rotation_env(request): + """Shares the RotationSensor configuration""" + cfg = { + "name": "test_rotation_sensor", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [{"sensor_type": "RGBCamera", "rotation": [0, -90, 0]}], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + } + + global shared_rotation_env + + if shared_rotation_env is None: + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + shared_rotation_env = holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) + + shared_rotation_env.reset() + return shared_rotation_env + + +abuse_config = { + "name": "test_abuse_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "AbuseSensor", + } + ], + "control_scheme": 0, + "location": [1.5, 0, 9], + "rotation": [0, 0, 0], + }, + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "AbuseSensor", + } + ], + "control_scheme": 0, + "location": [2, 1.5, 8], + "rotation": [0, 0, 0], + }, + ], +} diff --git a/PythonAPI/test/holoocean/sensors/test_abuse_sensor.py b/PythonAPI/test/holoocean/sensors/test_abuse_sensor.py new file mode 100644 index 0000000000..59a5e08354 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_abuse_sensor.py @@ -0,0 +1,34 @@ +def test_abuse_uav(abuse_world): + """Test that the UAV's blades hitting the ground triggers the abuse sensor""" + # Collide the uav's blades with the ground and check for abuse. + abuse_world.agents["uav0"].teleport([0, 0, 1], [0, 180, 0]) + abused = False + + for _ in range(20): + if abuse_world.tick()["uav0"]["AbuseSensor"] == 1: + abused = True + + assert abused, "The abuse sensor didn't trigger from uav blade collision!" + + +def test_abuse_turtle(abuse_world): + """Test that the turtle is abused when it is flipped over""" + + # Flip the turtle and check if it's abused + abuse_world.agents["turtle0"].teleport([0, 0, 1], [0, 180, 0]) + abuse_world.tick(20) + assert ( + abuse_world.tick()["turtle0"]["AbuseSensor"] == 1 + ), "The abuse sensor didn't trigger from the turtle agent flipping over" + + +def test_abuse_falling(agent_abuse_world): + """Test that the uav, turtle, and android are abused when they are dropped from a height""" + agent, abuse_world = agent_abuse_world + + abused = False + for _ in range(100): + if abuse_world.tick()[agent]["AbuseSensor"] == 1: + abused = True + + assert abused, "Agent {} was not abused!".format(agent) diff --git a/PythonAPI/test/holoocean/sensors/test_acoustic_beacon.py b/PythonAPI/test/holoocean/sensors/test_acoustic_beacon.py new file mode 100644 index 0000000000..754d4b9564 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_acoustic_beacon.py @@ -0,0 +1,189 @@ +import holoocean +import uuid +import numpy as np +import pytest + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "PerfectAUV", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "sensor_name": "Zero", + "location": [0, 0, 0], + "configuration": {"id": 0}, + }, + { + "sensor_type": "AcousticBeaconSensor", + "sensor_name": "One", + "location": [1, 0, 0], + "configuration": {"id": 1}, + }, + { + "sensor_type": "AcousticBeaconSensor", + "sensor_name": "Two", + "location": [0, 100, 0], + "configuration": {"id": 2}, + }, + ], + "control_scheme": 0, + "location": [0.0, 0.0, 5.0], + "rotation": [0.0, 0.0, 0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_sending(env): + """Make sure our sensor rates are working properly""" + env.reset() + + # send a message + env.send_acoustic_message(0, 1, "OWAY", "my_message") + state = env.tick() + + assert "One" in state + assert state["One"] == ["OWAY", 0, "my_message"] + + +@pytest.mark.parametrize("num", range(5)) +def test_timing(env, num): + # do random distance + dist = np.random.uniform(0, 1000) + env._scenario["agents"][0]["sensors"][2]["location"] = [0, dist, 0] + num_ticks = int(np.round(dist * 30 / (1500))) + + env.reset() + + # send a message + env.send_acoustic_message(0, 2, "OWAY", "my_message") + state = env.tick() + + for _ in range(num_ticks): + state = env.tick() + + assert "Two" in state + assert state["Two"] == ["OWAY", 0, "my_message"] + + +@pytest.mark.parametrize("num", range(5)) +def test_distance(env, num): + # do random distance + dist = np.random.uniform(0, 1000) + env._scenario["agents"][0]["sensors"][2]["location"] = [0, dist, 0] + num_ticks = int(np.round(dist * 30 / (1500))) + + env.reset() + + # send a message + env.send_acoustic_message(0, 2, "MSG_REQU", "my_message") + state = env.tick() + for _ in range(num_ticks): + state = env.tick() + + # get it back + state = env.tick() + for _ in range(num_ticks): + state = env.tick() + + assert "Zero" in state + # TODO determine what we want it to send back in these scenarios + assert state["Zero"][0:3] == ["MSG_RESPU", 2, None] + assert np.isclose(state["Zero"][5], dist) + + +def test_all_to_one(env): + env._scenario["agents"][0]["sensors"][2]["location"] = [0, 100, 0] + + env.reset() + + # send a message + env.send_acoustic_message(0, 2, "OWAY", "my_message") + state = env.tick() + env.send_acoustic_message(1, 2, "OWAY", "my_message") + + for _ in range(10): + state = env.tick() + assert "Two" not in state + + assert env.beacons_status == ["Idle"] * 3 + + +def test_one_to_all(env): + env._scenario["agents"][0]["sensors"][2]["location"] = [0, 100, 0] + + # send a message + env.send_acoustic_message(0, -1, "OWAY", "my_message") + + one = False + two = False + for _ in range(10): + state = env.tick() + + one = one or "One" in state + two = two or "Two" in state + + if one and two: + break + else: + assert env.beacons_status[0] == "Transmitting" + + assert one + assert two + assert env.beacons_status == ["Idle"] * 3 + + +@pytest.mark.parametrize( + "data", + zip( + [[1, 0], [0, 1], [-1, 0], [0, -1], [1, 1]], + [0, np.pi / 2, np.pi, -np.pi / 2, np.pi / 4], + ), +) +def test_azimuth(env, data): + xy, angle = data + env._scenario["agents"][0]["sensors"][2]["location"] = [xy[0], xy[1], 0] + + env.reset() + + # send a message + env.send_acoustic_message(2, 0, "OWAYU", "my_message") + state = env.tick() + + assert np.isclose(state["Zero"][3], angle) + + +@pytest.mark.parametrize( + "data", + zip([[1, 0], [1, 1], [1, -1], [-1, -1]], [0, np.pi / 4, -np.pi / 4, -np.pi / 4]), +) +def test_elevation(env, data): + yz, angle = data + env._scenario["agents"][0]["sensors"][2]["location"] = [0, yz[0], yz[1]] + + env.reset() + + # send a message + env.send_acoustic_message(2, 0, "OWAYU", "my_message") + state = env.tick() + + assert np.isclose(state["Zero"][4], angle) diff --git a/PythonAPI/test/holoocean/sensors/test_acoustic_beacon_line.py b/PythonAPI/test/holoocean/sensors/test_acoustic_beacon_line.py new file mode 100644 index 0000000000..efb0969e15 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_acoustic_beacon_line.py @@ -0,0 +1,185 @@ +import holoocean +import uuid +import numpy as np +import pytest + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "PerfectAUV", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "location": [0, 0, 0], + "configuration": {"id": 0}, + } + ], + "control_scheme": 0, + "location": [0.0, 0.0, 5.0], + "rotation": [0.0, 0.0, 0], + }, + { + "agent_name": "auv1", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "location": [0, 0, 0], + "configuration": {"id": 1}, + } + ], + "control_scheme": 0, + "location": [5.0, 0.0, 5.0], + "rotation": [0.0, 0.0, 0], + }, + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_within_max_distance(env): + "Tests to make sure that two sensors that are not within max distance do not transmit." + + env._scenario["agents"] = [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "location": [0, 0, 0], + "configuration": {"MaxDistance": 1}, + } + ], + "control_scheme": 0, + "location": [0.0, 0.0, 5.0], + "rotation": [0.0, 0.0, 0], + }, + { + "agent_name": "auv1", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "location": [0, 0, 0], + "configuration": {"MaxDistance": 1}, + } + ], + "control_scheme": 0, + "location": [5.0, 0.0, 5.0], + "rotation": [0.0, 0.0, 0], + }, + ] + + env.reset() + + for i in range(20): + env.send_acoustic_message(0, 1, "OWAY", "my_message") + state = env.tick() + assert ( + "AcousticBeaconSensor" not in state["auv1"] + ), "Receiving beacon received data when it should not have done so." + + +def test_obstructed_view(env): + """Tests to ensure that modem is unable to transmit when there is an obstruction between modems.""" + env._scenario["agents"] = [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "configuration": {"CheckVisible": True}, + } + ], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, 0], + }, + { + "agent_name": "auv1", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "configuration": {"CheckVisible": True}, + } + ], + "control_scheme": 1, + "location": [-10, 2, 0.25], + "rotation": [0, 0, 0], + }, + ] + + env.reset() + + for _ in range(20): + env.send_acoustic_message(0, 1, "OWAY", "my_message") + state = env.tick() + assert ( + "AcousticBeaconSensor" not in state["auv1"] + ), "Receiving beacon received data when it should not have done so." + + +def test_distance_noise(env): + """Tests to ensure that noise generation for max distance is functional""" + num_tests = 50 + tests_passed = 0 + + env._scenario["agents"] = [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "AcousticBeaconSensor", + "configuration": {"MaxDistance": 3, "DistanceSigma": 1}, + } + ], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "auv1", + "agent_type": "HoveringAUV", + "sensors": [{"sensor_type": "AcousticBeaconSensor"}], + "control_scheme": 1, + "location": [-10, 7, 0.25], + "rotation": [0, 0, 90], + }, + ] + + env.reset() + + for _ in range(num_tests): + env.send_acoustic_message(0, 1, "OWAY", "my_message") + state = env.tick() + + if "AcousticBeaconSensor" in state["auv1"]: + tests_passed += 1 + + assert ( + tests_passed < num_tests + ), "All messages sent when some should have failed due to noise variation." + assert ( + tests_passed > 0 + ), "All messages failed when some should have passed due to noise variation." diff --git a/PythonAPI/test/holoocean/sensors/test_ball_location_and_reward.py b/PythonAPI/test/holoocean/sensors/test_ball_location_and_reward.py new file mode 100644 index 0000000000..bb99b1c522 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_ball_location_and_reward.py @@ -0,0 +1,58 @@ +import holoocean +import uuid +import pytest + +cfg = { + "name": "test_ball_location_and_reward", + "world": "CupGame", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "CupGameTask", + "configuration": {"Speed": 3, "NumShuffles": 3, "Seed": 0}, + }, + {"sensor_type": "BallLocationSensor"}, + ], + "control_scheme": 0, + "location": [-0.4, -0.9, 1.8], + "rotation": [90, 0, 0], + } + ], + "window_width": 1024, + "window_height": 1024, +} + + +@pytest.mark.skipif( + "Dexterity" not in holoocean.installed_packages(), + reason="Dexterity package not installed", +) +def test_ball_location_and_reward(): + """Shuffle the ball using a seed. Ensure that after shuffling the ball location sensor + detects the correct position and move the sphere agent forward to collide with the correct cup. + Make sure it receives a reward of 1. + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("Dexterity") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + env.reset() + + env.tick(300) + for _ in range(30): + state = env.step([0]) + reward, terminal = env.get_reward_terminal() + if reward == 1: + touched_cup = True + assert touched_cup and state["BallLocationSensor"] == 2 diff --git a/PythonAPI/test/holoocean/sensors/test_clean_up_reward.py b/PythonAPI/test/holoocean/sensors/test_clean_up_reward.py new file mode 100644 index 0000000000..af77403330 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_clean_up_reward.py @@ -0,0 +1,56 @@ +import holoocean +import uuid +import pytest + +cfg = { + "name": "test_clean_up_reward", + "world": "CleanUp", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "LocationSensor", + }, + { + "sensor_type": "CleanUpTask", + "configuration": { + "NumTrash": 5, + "UseTable": True, + }, + }, + ], + "control_scheme": 0, + "location": [1, 1, 1], + } + ], +} + + +@pytest.mark.skipif( + "Dexterity" not in holoocean.installed_packages(), + reason="Dexterity package not installed", +) +def test_ball_location_and_reward(): + """This is currently a stub test. There is no way to reliably test the trash world so this is just meant to a manual + test where the tester makes sure that trash is spawning on a table. For now the reward and terminal should be zero. + Eventually there should be a way to add a debug option in the config so it can be tested programmatically. + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("Dexterity") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + env.reset() + + # env.agents["sphere0"].sensors["CleanUpTask"].start_task(4, False) + env.tick(100) + assert True diff --git a/PythonAPI/test/holoocean/sensors/test_collision_sensor.py b/PythonAPI/test/holoocean/sensors/test_collision_sensor.py new file mode 100644 index 0000000000..2d4d03809c --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_collision_sensor.py @@ -0,0 +1,62 @@ +import holoocean +import uuid + +uav_config = { + "name": "test_collision_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "CollisionSensor", + } + ], + "control_scheme": 0, + "location": [0, 0, 3], + } + ], +} + + +def test_collision_sensor_uav_falling(): + """Tests the collision sensor as the UAV falls, makes sure it fires when it hits the ground, + and it turns off when the UAV goes flying into the air + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=uav_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + collided = env.tick()["CollisionSensor"][0] + + assert ( + not collided + ), "The UAV is in the air but reported it collided with something!" + + for _ in range(80): + if env.tick()["CollisionSensor"][0]: + collided = True + + assert collided, "The UAV never reported it hit the ground!" + + # Make sure it stays colliding after it hits the ground + + for _ in range(30): + assert env.tick()["CollisionSensor"][0], "The UAV stopped colliding!" + + # Make sure it resets to not collided after shooting it up in the air + env.step([0, 0, 0, 100]) + + for _ in range(50): + env.tick() + + assert not env.tick()["CollisionSensor"][0], "The UAV is still colliding?" diff --git a/PythonAPI/test/holoocean/sensors/test_depth_sensor.py b/PythonAPI/test/holoocean/sensors/test_depth_sensor.py new file mode 100644 index 0000000000..2680d94ccd --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_depth_sensor.py @@ -0,0 +1,89 @@ +import holoocean +import uuid +from copy import deepcopy +import numpy as np + +from tests.utils.equality import almost_equal + +uav_config = { + "name": "test_depth_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "DepthSensor", + }, + { + "sensor_type": "DepthSensor", + "sensor_name": "noise", + "configuration": {"Sigma": 10}, + }, + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +def test_depth_sensor_falling(): + """Makes sure that the depth sensor updates as the UAV falls, and after it comes to a rest""" + cfg = deepcopy(uav_config) + + # Spawn the UAV 10 meters up + cfg["agents"][0]["location"] = [0, 0, 20] + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + last_location = env.tick()["DepthSensor"] + + for _ in range(85): + new_location = env.tick()["DepthSensor"] + assert ( + new_location[0] < last_location[0] + ), "UAV's location sensor did not detect falling!" + last_location = new_location + + # Give the UAV time to bounce and settle + for _ in range(80): + env.tick() + + # Make sure it is stationary now + last_location = env.tick()["DepthSensor"] + new_location = env.tick()["DepthSensor"] + + assert almost_equal( + last_location, new_location + ), "The UAV did not seem to settle!" + + +def test_depth_sensor_noise(): + """Make sure turning on noise actually turns it on""" + + config = deepcopy(uav_config) + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # let it land and then start moving forward + for _ in range(100): + state = env.tick() + assert not np.allclose(state["DepthSensor"], state["noise"]) diff --git a/PythonAPI/test/holoocean/sensors/test_dvl_sensor.py b/PythonAPI/test/holoocean/sensors/test_dvl_sensor.py new file mode 100644 index 0000000000..ec5627afbb --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_dvl_sensor.py @@ -0,0 +1,160 @@ +import holoocean +import uuid + +import numpy as np + +turtle_config = { + "name": "test_velocity_sensor", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "DVLSensor", + }, + { + "sensor_type": "DVLSensor", + "sensor_name": "noise", + "configuration": { + "VelSigma": 5, + "RangeSigma": 5, + }, + }, + ], + "control_scheme": 0, + "location": [-1.5, -1.50, 3.0], + } + ], +} + + +def test_dvl_sensor_straight(): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + turtle_config["agents"][0]["rotation"] = [0, 0, 0] + + with holoocean.environments.HoloOceanEnvironment( + scenario=turtle_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # let it land and then start moving forward + for _ in range(200): + last_x_velocity, _, _ = env.tick()["DVLSensor"][:3] + env.step([100, 0]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(30): + new_x_velocity, y_velocity, z_velocity = env.step([100, 0])["DVLSensor"][:3] + assert ( + new_x_velocity >= last_x_velocity + ), f"The velocity didn't increase at step {i}!" + assert y_velocity <= 0.5 + assert z_velocity <= 0.5 + last_x_velocity = new_x_velocity + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(30): + new_x_velocity, y_velocity, z_velocity = env.step([-100, 0])["DVLSensor"][ + :3 + ] + assert ( + new_x_velocity <= last_x_velocity + ), f"The velocity didn't decrease at step {i}!" + assert y_velocity <= 0.5 + assert z_velocity <= 0.5 + last_x_velocity = new_x_velocity + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_velocity, y_velocity, z_velocity = env.tick()["DVLSensor"][:3] + assert x_velocity <= 1e-2, "The x velocity wasn't close enough to zero!" + assert y_velocity <= 1e-2, "The y velocity wasn't close enough to zero!" + assert z_velocity <= 1e-2, "The z velocity wasn't close enough to zero!" + + +def test_dvl_sensor_rotated(): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + turtle_config["agents"][0]["rotation"] = [0, 0, 90] + + with holoocean.environments.HoloOceanEnvironment( + scenario=turtle_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # let it land and then start moving forward + for _ in range(100): + last_x_velocity, _, _ = env.tick()["DVLSensor"][:3] + env.step([100, 0]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(22): + new_x_velocity, y_velocity, z_velocity = env.step([100, 0])["DVLSensor"][:3] + assert ( + new_x_velocity >= last_x_velocity + ), f"The velocity didn't increase at step {i}!" + assert y_velocity <= 0.5 + assert z_velocity <= 0.5 + last_x_velocity = new_x_velocity + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(20): + new_x_velocity, y_velocity, z_velocity = env.step([-100, 0])["DVLSensor"][ + :3 + ] + assert ( + new_x_velocity <= last_x_velocity + ), f"The velocity didn't decrease at step {i}!" + assert y_velocity <= 0.5 + assert z_velocity <= 0.5 + last_x_velocity = new_x_velocity + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_velocity, y_velocity, z_velocity = env.tick()["DVLSensor"][:3] + assert x_velocity <= 1e-2, "The x velocity wasn't close enough to zero!" + assert y_velocity <= 1e-2, "The y velocity wasn't close enough to zero!" + assert z_velocity <= 1e-2, "The z velocity wasn't close enough to zero!" + + +def test_dvl_noise(): + """Make sure turning on noise actually turns it on""" + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + turtle_config["agents"][0]["rotation"] = [0, 0, 0] + + with holoocean.environments.HoloOceanEnvironment( + scenario=turtle_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # let it land and then start moving forward + for _ in range(10): + state = env.tick() + assert not np.allclose(state["DVLSensor"], state["noise"]) diff --git a/PythonAPI/test/holoocean/sensors/test_dynamics_sensor.py b/PythonAPI/test/holoocean/sensors/test_dynamics_sensor.py new file mode 100644 index 0000000000..c33dc97aa3 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_dynamics_sensor.py @@ -0,0 +1,185 @@ +import holoocean +import uuid +import pytest + +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_imu_sensor", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "DynamicsSensor", + "socket": "ViewPort", + "configuration": {"UseCOM": True, "UseRPY": True}, + }, + ], + "control_scheme": 0, + "location": [-1.5, -1.50, 1.0], + "rotation": [0, 0, 0], + }, + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "DynamicsSensor", + } + ], + "control_scheme": 0, + "location": [0, 0, 20], + }, + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_acceleration(env): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + _, _, _ = env.tick()["turtle0"]["DynamicsSensor"][0:3] + env.step([100, 0]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(30): + x_accel, y_accel, z_accel = env.step([100, 0])["turtle0"]["DynamicsSensor"][0:3] + assert x_accel >= 0, f"The acceleration wasn't positive at step {i}!" + assert y_accel <= 0.5 + assert z_accel <= 0.5 + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(30): + x_accel, y_accel, z_accel = env.step([-100, 0])["turtle0"]["DynamicsSensor"][ + 0:3 + ] + assert x_accel <= 1e-3, f"The acceleration wasn't negative at step {i}!" + assert y_accel <= 0.5 + assert z_accel <= 0.5 + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_accel, y_accel, z_accel = env.tick()["turtle0"]["DynamicsSensor"][0:3] + assert x_accel <= 1e-2, "The x accel wasn't close enough to zero!" + assert y_accel <= 1e-2, "The y accel wasn't close enough to zero!" + + +def test_angular_velocity(env): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + _, _, last_z_angvel = env.tick()["turtle0"]["DynamicsSensor"][9:12] + env.step([0, 25]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(60): + x_angvel, y_angvel, new_z_angvel = env.step([0, 25])["turtle0"][ + "DynamicsSensor" + ][9:12] + assert x_angvel <= 0.5 + assert y_angvel <= 0.5 + assert new_z_angvel >= last_z_angvel, f"The angvel didn't increase at step {i}!" + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(60): + x_angvel, y_angvel, new_z_angvel = env.step([0, -25])["turtle0"][ + "DynamicsSensor" + ][9:12] + assert x_angvel <= 0.5 + assert y_angvel <= 0.5 + assert new_z_angvel <= last_z_angvel, f"The angvel didn't decrease at step {i}!" + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_angvel, y_angvel, z_angvel = env.tick()["turtle0"]["DynamicsSensor"][9:12] + assert x_angvel <= 1e-2, "The x angvel wasn't close enough to zero!" + assert y_angvel <= 1e-2, "The y angvel wasn't close enough to zero!" + assert z_angvel <= 1e-2, "The z angvel wasn't close enough to zero!" + + +def test_velocity(env): + """Drop the UAV, make sure the z velocity is increasingly negative as it falls. + Make sure it zeros out after it hits the ground, and then goes positive on takeoff + """ + + env.reset() + + last_z_velocity = env.tick()["uav0"]["DynamicsSensor"][5] + + for _ in range(50): + new_z_velocity = env.tick()["uav0"]["DynamicsSensor"][5] + assert new_z_velocity <= last_z_velocity, "The velocity didn't decrease!" + last_z_velocity = new_z_velocity + + # Make sure it hits the ground + for _ in range(60): + env.tick() + + last_z_velocity = env.tick()["uav0"]["DynamicsSensor"][5] + + assert last_z_velocity <= 1e-4, "The velocity wasn't close enough to zero!" + + # Send it flying up into the air to make sure the z velocity increases + env.act("uav0", [0, 0, 0, 100]) + env.tick() + + # z velocity should be positive now + for _ in range(20): + new_z_velocity = env.tick()["uav0"]["DynamicsSensor"][5] + assert new_z_velocity >= last_z_velocity, "The velocity didn't increase!" + last_z_velocity = new_z_velocity + + +def test_params(env): + env.reset() + + before = env.tick()["turtle0"]["DynamicsSensor"] + assert before.shape == (18,) + + env._scenario["agents"][0]["sensors"][0]["configuration"]["UseRPY"] = False + env._scenario["agents"][0]["sensors"][0]["configuration"]["UseCOM"] = False + + env.reset() + + after = env.tick()["turtle0"]["DynamicsSensor"] + # Location should be different since we're not at COM + # Should check orientation as well, but we've change how it's saved + assert not np.allclose(after[6:9], before[6:9]) diff --git a/PythonAPI/test/holoocean/sensors/test_gps_sensor.py b/PythonAPI/test/holoocean/sensors/test_gps_sensor.py new file mode 100644 index 0000000000..e0e8aa7cee --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_gps_sensor.py @@ -0,0 +1,75 @@ +import holoocean +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "sphere", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere", + "agent_type": "SphereAgent", + "sensors": [{"sensor_type": "GPSSensor", "configuration": {}}], + "control_scheme": 0, + "location": [0, 0, 0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(3)) +def test_setting_depth(env, num): + """Make sure if it's above the depth we receive data, and if below we don't""" + depth = np.random.rand() * 10 + env._scenario["agents"][0]["sensors"][0]["configuration"]["Depth"] = depth + env._scenario["agents"][0]["location"] = [0, 0, -1 * depth + 1] + + env.reset() + + state = env.tick() + assert "GPSSensor" in state + + # Test below + env._scenario["agents"][0]["location"] = [0, 0, -1 * depth - 1] + + env.reset() + + state = env.tick() + assert "GPSSensor" not in state + + +@pytest.mark.parametrize("num", range(3)) +def test_random_depth(env, num): + """Make sure the depth is changing according to the noise we put in""" + num_ticks = 100 + depth = np.random.rand() * 10 + env._scenario["agents"][0]["sensors"][0]["configuration"]["Depth"] = depth + env._scenario["agents"][0]["sensors"][0]["configuration"]["DepthSigma"] = 1 + env._scenario["agents"][0]["location"] = [0, 0, -1 * depth] + + env.reset() + + count = 0 + for _ in range(num_ticks): + state = env.tick() + if "GPSSensor" in state: + count += 1 + + assert 0 < count + assert count < num_ticks diff --git a/PythonAPI/test/holoocean/sensors/test_imu_sensor.py b/PythonAPI/test/holoocean/sensors/test_imu_sensor.py new file mode 100644 index 0000000000..2e64a66da7 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_imu_sensor.py @@ -0,0 +1,187 @@ +import holoocean +import uuid +import pytest + +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_imu_sensor", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "IMUSensor", + }, + { + "sensor_type": "IMUSensor", + "sensor_name": "noise", + "configuration": { + "AccelSigma": 10, + "AngVelSigma": 10, + "ReturnBias": True, + }, + }, + ], + "control_scheme": 0, + "location": [-1.5, -1.50, 1.0], + "rotation": [0, 0, 0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_imu_sensor_acceleration(env): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + _, _, _ = env.tick()["IMUSensor"][0] + env.step([100, 0]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(30): + x_accel, y_accel, z_accel = env.step([100, 0])["IMUSensor"][0] + assert x_accel >= 0, f"The acceleration wasn't positive at step {i}!" + assert y_accel <= 0.5 + assert np.isclose(z_accel, 9.81, 1e-2), f"The z_accel wasn't 9.81 at step {i}!" + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(30): + x_accel, y_accel, z_accel = env.step([-100, 0])["IMUSensor"][0] + assert x_accel <= 1e-3, f"The acceleration wasn't negative at step {i}!" + assert y_accel <= 0.5 + assert np.isclose(z_accel, 9.81, 1e-2) + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_accel, y_accel, z_accel = env.tick()["IMUSensor"][0] + assert x_accel <= 1e-2, "The x accel wasn't close enough to zero!" + assert y_accel <= 1e-2, "The y accel wasn't close enough to zero!" + + +def test_imu_sensor_angular_velocity(env): + """Make sure when we move forward x is positive, y is close to 0. Make sure when not moving both are close to 0""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + _, _, last_z_angvel = env.tick()["IMUSensor"][1] + env.step([0, 25]) + + # Move forward, making sure y is relatively small, and x is increasing + for i in range(60): + x_angvel, y_angvel, new_z_angvel = env.step([0, 25])["IMUSensor"][1] + assert x_angvel <= 0.5 + assert y_angvel <= 0.5 + assert new_z_angvel >= last_z_angvel, f"The angvel didn't increase at step {i}!" + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # Move backward, making sure y is relatively small, and x is decreasing + for i in range(60): + x_angvel, y_angvel, new_z_angvel = env.step([0, -25])["IMUSensor"][1] + assert x_angvel <= 0.5 + assert y_angvel <= 0.5 + assert new_z_angvel <= last_z_angvel, f"The angvel didn't decrease at step {i}!" + + # Let it stop + for _ in range(100): + env.step([0, 0]) + + # make sure everythign is close to 0 + x_angvel, y_angvel, z_angvel = env.tick()["IMUSensor"][1] + assert x_angvel <= 1e-2, "The x angvel wasn't close enough to zero!" + assert y_angvel <= 1e-2, "The y angvel wasn't close enough to zero!" + assert z_angvel <= 1e-2, "The z angvel wasn't close enough to zero!" + + +def test_imu_noise_accel(env): + """Make sure turning on noise actually turns it on""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + state = env.tick() + assert not np.allclose(state["IMUSensor"][0], state["noise"][0]) + + +def test_imu_noise_angvel(env): + """Make sure turning on noise actually turns it on""" + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + state = env.tick() + assert not np.allclose(state["IMUSensor"][1], state["noise"][1]) + + +def test_imu_noise_returnbias(env): + """Make sure returning the bias actually works""" + + env.reset() + + # let it land and then start moving forward + for _ in range(10): + state = env.tick() + assert state["IMUSensor"].shape == (2, 3) + assert state["noise"].shape == (4, 3) + assert np.allclose(np.zeros(3), state["noise"][2]) + assert np.allclose(np.zeros(3), state["noise"][3]) + + +def test_imu_noise_bias_accel(env): + """Make sure turning on noise actually turns it on""" + + env._scenario["agents"][0]["sensors"][1]["configuration"]["AccelBiasSigma"] = 10 + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + state = env.tick() + assert not np.allclose(np.zeros(3), state["noise"][2]) + + +def test_imu_noise_bias_angvel(env): + """Make sure turning on noise actually turns it on""" + + env._scenario["agents"][0]["sensors"][1]["configuration"]["AngVelBiasSigma"] = 10 + + env.reset() + + # let it land and then start moving forward + for _ in range(100): + state = env.tick() + assert not np.allclose(np.zeros(3), state["noise"][3]) diff --git a/PythonAPI/test/holoocean/sensors/test_location_sensor.py b/PythonAPI/test/holoocean/sensors/test_location_sensor.py new file mode 100644 index 0000000000..4b1a5dedc2 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_location_sensor.py @@ -0,0 +1,116 @@ +import holoocean +import uuid +from copy import deepcopy + +from tests.utils.equality import almost_equal + +sphere_config = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "LocationSensor", + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +def test_location_sensor_after_teleport(): + """Make sure the location sensor updates after a teleport. Also verifies that the coordinates for the teleport + command match the coordinates used by the location sensor + """ + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=sphere_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(10): + env.tick() + + loc = [507, 301, 1620] + env.agents["sphere0"].teleport(loc, [0, 0, 0]) + + state = env.tick() + sensed_loc = state["LocationSensor"] + + assert almost_equal( + loc, sensed_loc + ), "Sensed location did not match the expected location!" + + +uav_config = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "LocationSensor", + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +def test_location_sensor_falling(): + """Makes sure that the location sensor updates as the UAV falls, and after it comes to a rest""" + cfg = deepcopy(uav_config) + + # Spawn the UAV 10 meters up + cfg["agents"][0]["location"] = [0, 0, 20] + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + last_location = env.tick()["LocationSensor"] + + for _ in range(85): + new_location = env.tick()["LocationSensor"] + assert ( + new_location[2] < last_location[2] + ), "UAV's location sensor did not detect falling!" + last_location = new_location + + # Give the UAV time to bounce and settle + for _ in range(80): + env.tick() + + # Make sure it is stationary now + last_location = env.tick()["LocationSensor"] + new_location = env.tick()["LocationSensor"] + + assert almost_equal( + last_location, new_location + ), "The UAV did not seem to settle!" + + +###################################################### +# LocationSensor noise tests are found in test_mvn.py +###################################################### diff --git a/PythonAPI/test/holoocean/sensors/test_magnetometer_sensor.py b/PythonAPI/test/holoocean/sensors/test_magnetometer_sensor.py new file mode 100644 index 0000000000..a5f5d62944 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_magnetometer_sensor.py @@ -0,0 +1,76 @@ +import holoocean +import uuid +import pytest +import numpy as np + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "sphere", + "agents": [ + { + "agent_name": "sphere", + "agent_type": "SphereAgent", + "sensors": [ + {"sensor_type": "MagnetometerSensor", "configuration": {}}, + {"sensor_type": "OrientationSensor"}, + ], + "control_scheme": 0, + "location": [0, 0, 0], + "rotation": [0, 0, 0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(3)) +def test_setting_vector(env, num): + """Make sure setting the vector gets us what we want""" + vec = np.random.rand(3) + vec /= np.linalg.norm(vec) + env._scenario["agents"][0]["sensors"][0]["configuration"][ + "MagneticVector" + ] = vec.tolist() + env._scenario["agents"][0]["rotation"] = (np.random.rand(3) * 30).tolist() + + env.reset() + + state = env.tick() + + expected = state["OrientationSensor"].T @ vec + + assert np.allclose(expected, state["MagnetometerSensor"], rtol=1e-3) + + +@pytest.mark.parametrize("num", range(3)) +def test_random(env, num): + """Make sure enabling noise perturbs things""" + vec = np.random.rand(3) + vec /= np.linalg.norm(vec) + env._scenario["agents"][0]["sensors"][0]["configuration"][ + "MagneticVector" + ] = vec.tolist() + env._scenario["agents"][0]["rotation"] = (np.random.rand(3) * 30).tolist() + + env._scenario["agents"][0]["sensors"][0]["configuration"]["Sigma"] = 10 + + env.reset() + + state = env.tick() + + expected = state["OrientationSensor"].T @ vec + + assert not np.allclose(expected, state["MagnetometerSensor"]) diff --git a/PythonAPI/test/holoocean/sensors/test_mvn.py b/PythonAPI/test/holoocean/sensors/test_mvn.py new file mode 100644 index 0000000000..faf7362880 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_mvn.py @@ -0,0 +1,139 @@ +import holoocean +import pytest +import uuid + +from scipy.stats import multivariate_normal as mvn +import numpy as np + +eps = 1e-9 + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_mvn", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "LocationSensor", + "sensor_name": "noise", + "configuration": {}, + }, + {"sensor_type": "LocationSensor", "sensor_name": "clean"}, + ], + "control_scheme": 0, + "location": [-1.5, -1.50, 3.0], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(5)) +def test_sigma_scalar(env, num): + """Make sure it can take a plain std as a parameter""" + # Generate a sigma + sigma = np.random.rand() * 10 + cov = np.diag([sigma, sigma, sigma]) ** 2 + + env._scenario["agents"][0]["sensors"][0]["configuration"].pop("Cov", None) + env._scenario["agents"][0]["sensors"][0]["configuration"]["Sigma"] = sigma + + env.reset() + + for i in range(5): + state = env.tick() + + p = mvn.pdf(state["noise"], mean=state["clean"], cov=cov) + assert p > eps + + +@pytest.mark.parametrize("num", range(5)) +def test_sigma_vector(env, num): + """Make sure it can take diagonal array as std""" + # Generate a sigma + sigma = np.random.rand(3) * 10 + cov = np.diag(sigma**2) + + env._scenario["agents"][0]["sensors"][0]["configuration"].pop("Cov", None) + env._scenario["agents"][0]["sensors"][0]["configuration"]["Sigma"] = list(sigma) + + env.reset() + + for i in range(5): + state = env.tick() + + p = mvn.pdf(state["noise"], mean=state["clean"], cov=cov) + assert p > eps + + +@pytest.mark.parametrize("num", range(5)) +def test_cov_num(env, num): + """Make sure it can take covariance as a scalar""" + # Generate a sigma + cov_num = np.random.rand() * 10 + cov = np.diag([cov_num, cov_num, cov_num]) + + env._scenario["agents"][0]["sensors"][0]["configuration"].pop("Sigma", None) + env._scenario["agents"][0]["sensors"][0]["configuration"]["Cov"] = cov_num + + env.reset() + + for i in range(5): + state = env.tick() + + p = mvn.pdf(state["noise"], mean=state["clean"], cov=cov) + assert p > eps + + +@pytest.mark.parametrize("num", range(5)) +def test_cov_vector(env, num): + """Make sure it can take diagonal array as cov""" + # Generate a cov + cov_array = np.random.rand(3) * 10 + cov = np.diag(cov_array) + + env._scenario["agents"][0]["sensors"][0]["configuration"].pop("Sigma", None) + env._scenario["agents"][0]["sensors"][0]["configuration"]["Cov"] = list(cov_array) + + env.reset() + + for i in range(5): + state = env.tick() + + p = mvn.pdf(state["noise"], mean=state["clean"], cov=cov) + assert p > eps + + +@pytest.mark.parametrize("num", range(5)) +def test_cov_matrix(env, num): + """Make sure it can take a full matrix as covariance""" + # Generate a cov + cov = np.random.rand(3, 3) * 10 + cov = cov @ cov.T + + env._scenario["agents"][0]["sensors"][0]["configuration"].pop("Sigma", None) + env._scenario["agents"][0]["sensors"][0]["configuration"]["Cov"] = cov.tolist() + + env.reset() + + for i in range(5): + state = env.tick() + + p = mvn.pdf(state["noise"], mean=state["clean"], cov=cov) + assert p > eps diff --git a/PythonAPI/test/holoocean/sensors/test_optical_modem_sensor.py b/PythonAPI/test/holoocean/sensors/test_optical_modem_sensor.py new file mode 100644 index 0000000000..947e338a33 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_optical_modem_sensor.py @@ -0,0 +1,283 @@ +import holoocean +import uuid +import pytest + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "TurtleAgent", + "sensors": [{"sensor_type": "OpticalModemSensor"}], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "uav1", + "agent_type": "TurtleAgent", + "sensors": [{"sensor_type": "OpticalModemSensor"}], + "control_scheme": 1, + "location": [-10, 7, 0.25], + "rotation": [0, 0, 90], + }, + ], + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + yield env + + +def test_transmittable(env): + env.reset() + + command = [0, 0] + for _ in range(20): + env.send_optical_message(0, 1, "Message") + state = env.step(command) + + assert ( + "OpticalModemSensor" in state["uav1"] + ), "Receiving modem did not receive data when it should have." + assert ( + state["uav1"]["OpticalModemSensor"] == "Message" + ), "Wrong message received." + + +def test_within_max_distance(env): + "Tests to make sure that two sensors that are not within max distance do not transmit." + + env._scenario["agents"] = [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + "configuration": { + "MaxDistance": 3, + }, + } + ], + "control_scheme": 1, + "location": [-5, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "uav1", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + "configuration": { + "MaxDistance": 3, + }, + } + ], + "control_scheme": 1, + "location": [-5, 3, 0.25], + "rotation": [0, 0, 90], + }, + ] + + env.reset() + + command = [0, 0, 10, 50] + for i in range(20): + env.send_optical_message(0, 1, "Message") + state = env.tick() + assert ( + "OpticalModemSensor" not in state["uav1"] + ), "Receiving modem received data when it should not have done so." + + +def test_not_oriented(env): + "Tests to make sure that two sensors that are not within max distance do not transmit." + + env._scenario["agents"] = [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [{"sensor_type": "OpticalModemSensor"}], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, 0], + }, + { + "agent_name": "uav1", + "agent_type": "UavAgent", + "sensors": [{"sensor_type": "OpticalModemSensor"}], + "control_scheme": 1, + "location": [-10, 7, 0.25], + "rotation": [0, 0, -180], + }, + ] + + env.reset() + + command = [0, 0, 10, 50] + for i in range(20): + env.send_optical_message(0, 1, "Message") + state = env.step(command) + assert ( + "OpticalModemSensor" not in state["uav1"] + ), "Receiving modem received data when it should not have done so." + + +def test_obstructed_view(env): + """Tests to ensure that modem is unable to transmit when there is an obstruction between modems.""" + env._scenario["agents"] = [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + } + ], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "uav1", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + } + ], + "control_scheme": 1, + "location": [-10, 2, 0.25], + "rotation": [0, 0, -180], + }, + ] + + env.reset() + + command = [0, 0, 10, 50] + + for _ in range(20): + env.send_optical_message(0, 1, "Message") + state = env.step(command) + assert ( + "OpticalModemSensor" not in state["uav1"] + ), "Receiving modem received data when it should not have done so." + + +def test_distance_noise(env): + """Tests to ensure that noise generation for max distance is functional""" + num_tests = 50 + tests_passed = 0 + + env._scenario["agents"] = [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + "configuration": {"MaxDistance": 3.8, "DistanceSigma": 1.5}, + } + ], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "uav1", + "agent_type": "UavAgent", + "sensors": [{"sensor_type": "OpticalModemSensor"}], + "control_scheme": 1, + "location": [-10, 7, 0.25], + "rotation": [0, 0, 90], + }, + ] + + env.reset() + + command = [0, 0, 10, 50] + for _ in range(num_tests): + env.send_optical_message(0, 1, "Message") + state = env.step(command) + + if ( + "OpticalModemSensor" in state["uav1"] + and state["uav1"]["OpticalModemSensor"] == "Message" + ): + tests_passed += 1 + + assert ( + tests_passed < num_tests + ), "All messages sent when some should have failed due to noise variation." + assert ( + tests_passed > 0 + ), "All messages failed when some should have passed due to noise variation." + + +def test_angle_noise(env): + """Tests to ensure that noise generation for max distance is functional""" + num_tests = 50 + tests_passed = 0 + + env._scenario["agents"] = [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + "configuration": {"MaxDistance": 20, "AngleSigma": 20}, + } + ], + "control_scheme": 1, + "location": [-10, 10, 0.25], + "rotation": [0, 0, -90], + }, + { + "agent_name": "uav1", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "OpticalModemSensor", + "configuration": {"MaxDistance": 20, "AngleSigma": 20}, + } + ], + "control_scheme": 1, + "location": [-15.2, 7, 0.25], + "rotation": [0, 0, 90], + }, + ] + + env.reset() + + command = [0, 0, 10, 50] + for _ in range(num_tests): + env.send_optical_message(0, 1, "Message") + state = env.step(command) + if ( + "OpticalModemSensor" in state["uav1"] + and state["uav1"]["OpticalModemSensor"] == "Message" + ): + tests_passed += 1 + + assert ( + tests_passed < num_tests + ), "All messages sent when some should have failed due to noise variation." + assert ( + tests_passed > 0 + ), "All messages failed when some should have passed due to noise variation." diff --git a/PythonAPI/test/holoocean/sensors/test_orientation_sensor.py b/PythonAPI/test/holoocean/sensors/test_orientation_sensor.py new file mode 100644 index 0000000000..da09ae7fcc --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_orientation_sensor.py @@ -0,0 +1,62 @@ +import holoocean +import numpy as np +from tests.utils.equality import almost_equal +import uuid +from scipy.spatial.transform import Rotation +import pytest +from scipy.linalg import logm + + +def rot_error(A, B): + e = logm(A @ B.T) + return np.array([e[0, 1], e[0, 2], e[1, 2]]) + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_orientation_sensor", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "OrientationSensor", + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(3)) +def test_orientation_sensor_after_teleport(env, num): + """Make sure that the orientation sensor correctly updates after using a teleport command to + rotate the agent + """ + loc = [123, 3740, 1030] + rot_deg = np.random.rand(3) * 45 + R = Rotation.from_euler("xyz", rot_deg, degrees=True).as_matrix() + + env.agents["sphere0"].teleport(loc, rot_deg) + state = env.tick() + sensed_orientation = state["OrientationSensor"] + + assert np.allclose( + np.zeros(3), rot_error(R, sensed_orientation), atol=1e-5 + ), "Expected orientation did not match the expected orientation!" diff --git a/PythonAPI/test/holoocean/sensors/test_pose_sensor.py b/PythonAPI/test/holoocean/sensors/test_pose_sensor.py new file mode 100644 index 0000000000..10f1ccc017 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_pose_sensor.py @@ -0,0 +1,54 @@ +import holoocean +import uuid +import numpy as np +from tests.utils.equality import almost_equal + +turtle_config = { + "name": "test_velocity_sensor", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [ + { + "sensor_type": "PoseSensor", + }, + { + "sensor_type": "OrientationSensor", + }, + { + "sensor_type": "LocationSensor", + }, + ], + "control_scheme": 0, + "location": [-1.5, -1.50, 3.0], + } + ], +} + + +def test_pose_sensor_straight(): + """Make sure pose sensor returns the same values as the orientation and location sensors""" + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=turtle_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + # let it land and then start moving forward + for _ in range(200): + command = np.random.random(size=2) + state = env.step(command) + assert almost_equal( + state["PoseSensor"][:3, :3], state["OrientationSensor"] + ), f"Rotation in PoseSensor doesn't match that in OrientationSensor at timestep {i}" + assert almost_equal( + state["PoseSensor"][:3, 3], state["LocationSensor"] + ), f"Location in PoseSensor doesn't match that in LocationSensor at timestep {i}" diff --git a/PythonAPI/test/holoocean/sensors/test_range_finder_sensor.py b/PythonAPI/test/holoocean/sensors/test_range_finder_sensor.py new file mode 100644 index 0000000000..a1aa2a540c --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_range_finder_sensor.py @@ -0,0 +1,108 @@ +import holoocean +import uuid + +from tests.utils.equality import almost_equal + + +sphere_config = { + "name": "test_range_finder_sensor", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "RangeFinderSensor", + "configuration": {"LaserMaxDistance": 1, "LaserCount": 12}, + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +def test_range_finder_sensor_max(): + """Make sure the range sensor set max distance correctly.""" + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=sphere_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + state = env.tick(10) + + actual = state["RangeFinderSensor"] + + expected_count = sphere_config["agents"][0]["sensors"][0]["configuration"][ + "LaserCount" + ] + assert ( + len(actual) == expected_count + ), "Sensed range size did not match the expected size!" + + expected_dist = sphere_config["agents"][0]["sensors"][0]["configuration"][ + "LaserMaxDistance" + ] + assert all(x > 0 for x in actual), "Sensed range includes 0!" + assert all( + x <= expected_dist for x in actual + ), "Sensed range includes value greater than 1!" + + +uav_config = { + "name": "test_range_finder_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "RangeFinderSensor", + "configuration": {"LaserAngle": -90, "LaserMaxDistance": 15}, + } + ], + "control_scheme": 0, + "location": [0, 0, 10], + } + ], +} + + +def test_range_finder_sensor_falling(): + """Makes sure that the range sensor updates as the UAV falls, and after it comes to a rest.""" + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=uav_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + last_range = env.tick()["RangeFinderSensor"][0] + + for _ in range(10): + new_range = env.tick(4)["RangeFinderSensor"][0] + assert new_range < last_range, "UAV's range sensor did not detect falling!" + last_range = new_range + + # Give the UAV time to bounce and settle + env.tick(80) + + # Make sure it is stationary now + last_range = env.tick()["RangeFinderSensor"][0] + new_range = env.tick()["RangeFinderSensor"][0] + + assert almost_equal(last_range, new_range), "The UAV did not seem to settle!" diff --git a/PythonAPI/test/holoocean/sensors/test_rates.py b/PythonAPI/test/holoocean/sensors/test_rates.py new file mode 100644 index 0000000000..ac09d275a6 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_rates.py @@ -0,0 +1,42 @@ +import holoocean +import uuid + +turtle_config = { + "name": "test_velocity_sensor", + "world": "TestWorld", + "main_agent": "turtle0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "turtle0", + "agent_type": "TurtleAgent", + "sensors": [{"sensor_type": "DVLSensor", "Hz": 15}], + "control_scheme": 0, + "location": [-1.5, -1.50, 3.0], + } + ], +} + + +def test_rates(): + """Make sure our sensor rates are working properly""" + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=turtle_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ticks_per_sec=30, + ) as env: + # see if it's in the first one (should be every other) + state = env.tick() + isin = "DVLSensor" in state + + # iterate through a bunch + for _ in range(100): + state = env.tick() + new = "DVLSensor" in state + assert isin != new + isin = new diff --git a/PythonAPI/test/holoocean/sensors/test_rgb_camera.py b/PythonAPI/test/holoocean/sensors/test_rgb_camera.py new file mode 100644 index 0000000000..7af7f69e2d --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_rgb_camera.py @@ -0,0 +1,158 @@ +import holoocean +import cv2 +import copy +import os +import uuid +import pytest + +from tests.utils.equality import mean_square_err + +base_cfg = { + "name": "test_rgb_camera", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "RGBCamera", + "socket": "CameraSocket", + # note the different camera name. Regression test for #197 + "sensor_name": "TestCamera", + } + ], + "control_scheme": 0, + "location": [ + 0.95, + -1.75, + 0.5, + ], # if you change this, you must change rotation_env too. + } + ], +} + + +@pytest.mark.skipif( + "TestWorlds" not in holoocean.installed_packages(), + reason="Ocean package not installed", +) +def test_rgb_camera(resolution, request): + """Makes sure that the RGB camera is positioned and capturing correctly. + + Capture pixel data, and load from disk the baseline of what it should look like. + Then, use mse() to see how different the images are. + + """ + + global base_cfg + + cfg = copy.deepcopy(base_cfg) + + cfg["agents"][0]["sensors"][0]["configuration"] = { + "CaptureWidth": resolution, + "CaptureHeight": resolution, + } + + # print("config: ", cfg) + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(5): + env.tick() + + pixels = env.tick()["TestCamera"][:, :, 0:3] + filepath = str("/baseline_images/baseline_" + str(resolution) + ".png") + baseline = cv2.imread(str(request.fspath.dirname + filepath)) + err = mean_square_err(pixels, baseline) + + # Uncomment to visually compare images + # cv2.imshow("Baselines", baseline) + # cv2.imshow("Pixels", pixels) + # cv2.waitKey(0) + assert err < 2000 + + +shared_ticks_per_capture_env = None + + +def make_ticks_per_capture_env(): + """test_rgb_camera_ticks_per_capture shares an environment with different + instances of the same test + """ + global base_cfg, shared_ticks_per_capture_env + + cfg = copy.deepcopy(base_cfg) + + cfg["agents"][0]["sensors"][0]["configuration"] = { + "CaptureWidth": 512, + "CaptureHeight": 512, + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + shared_ticks_per_capture_env = holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) + + +# def test_rgb_camera_ticks_per_capture(ticks_per_capture): +# """Validate that the ticks_per_capture method actually makes the RGBCamera take fewer +# screenshots. + +# Capture a screenshot, wait for it to change, then make sure that the image doesn't +# change until ticks_per_capture ticks have elapsed. + +# """ +# global shared_ticks_per_capture_env + +# if shared_ticks_per_capture_env is None: +# make_ticks_per_capture_env() + +# env = shared_ticks_per_capture_env +# env.reset() + +# # The agent needs to be moving for the image to change +# env.act("sphere0", [2]) + +# env.agents["sphere0"].sensors["TestCamera"].set_ticks_per_capture(ticks_per_capture) + +# # Take the initial capture, and wait until it changes +# initial = env.tick()['TestCamera'][:, :, 0:3] + +# MAX_TRIES = 50 +# tries = 0 + +# while tries < MAX_TRIES: +# intermediate = env.tick()['TestCamera'][:, :, 0:3] +# if mean_square_err(initial, intermediate) > 10: +# break +# tries += 1 + +# assert MAX_TRIES != tries, "Timed out waiting for the image to change!" + +# # On the last tick, intermediate changed. Now, it should take +# # ticks_per_capture ticks for it to change again. +# initial = intermediate +# for _ in range(ticks_per_capture - 1): +# # Make sure it doesn't change +# intermediate = env.tick()['TestCamera'][:, :, 0:3] +# assert mean_square_err(initial, intermediate) < 10, "The RGBCamera output changed unexpectedly!" + +# # Now it should change + +# final = env.tick()['TestCamera'][:, :, 0:3] +# assert mean_square_err(initial, final) > 10, "The RGBCamera output did not change when expected!" diff --git a/PythonAPI/test/holoocean/sensors/test_rotation_sensor.py b/PythonAPI/test/holoocean/sensors/test_rotation_sensor.py new file mode 100644 index 0000000000..b67d725069 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_rotation_sensor.py @@ -0,0 +1,54 @@ +import holoocean +import numpy as np +import uuid +import pytest + + +@pytest.fixture(scope="module") +def env(): + scenario = { + "name": "test_rotation_sensor", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [ + { + "sensor_type": "RotationSensor", + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + } + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=scenario, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + yield env + + +@pytest.mark.parametrize("num", range(3)) +def test_rotation_sensor_after_teleport(env, num): + """Make sure that the rotation sensor correctly updates after using a teleport command to + rotate the agent + + """ + loc = [123, 3740, 1030] + # Do much smaller rotations to avoid singularities + rot_deg = np.random.rand(3) * 20 + + env.agents["sphere0"].teleport(loc, rot_deg) + rotation = env.tick()["RotationSensor"] + + assert np.allclose( + rot_deg, rotation, rtol=1e-2 + ), "The agent rotated in an unexpected manner!" diff --git a/PythonAPI/test/holoocean/sensors/test_sensor_rotation.py b/PythonAPI/test/holoocean/sensors/test_sensor_rotation.py new file mode 100644 index 0000000000..f5d41b2e4e --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_sensor_rotation.py @@ -0,0 +1,44 @@ +import cv2, os +from tests.utils.equality import mean_square_err +import holoocean +import pytest + + +@pytest.mark.skipif( + "TestWorlds" not in holoocean.installed_packages(), + reason="Ocean package not installed", +) +def test_sensor_rotation(rotation_env, request): + """Validates that calling rotate actually rotates the sensor using the RGBCamera. + + Positions the SphereAgent above the target cube so that if it rotates down, it will capture the test + pattern. + """ + # Re-use the screenshot for test_rgb_camera + rotation_env.agents["sphere0"].sensors["RGBCamera"].rotate([0, 0, 0]) + pixels = rotation_env.tick(10)["RGBCamera"][:, :, 0:3] + + filepath = str("/baseline_images/baseline_256.png") + baseline = cv2.imread(str(request.fspath.dirname + filepath)) + + err = mean_square_err(pixels, baseline) + assert err < 2000, "The sensor appeared to not rotate!" + + +@pytest.mark.skipif( + "TestWorlds" not in holoocean.installed_packages(), + reason="Ocean package not installed", +) +def test_sensor_rotation_resets_after_reset(rotation_env): + """Validates that the sensor rotation is reset back to the starting position after calling ``.reset()``.""" + + # Re-use the screenshot for test_rgb_camera + rotation_env.agents["sphere0"].sensors["RGBCamera"].rotate([0, 0, 0]) + pixels_before = rotation_env.tick(5)["RGBCamera"][:, :, 0:3] + + rotation_env.reset() + + pixels_after = rotation_env.tick(5)["RGBCamera"][:, :, 0:3] + + err = mean_square_err(pixels_before, pixels_after) + assert err > 2000, "The images were too similar! Did the sensor not rotate back?" diff --git a/PythonAPI/test/holoocean/sensors/test_sonar_sensor.py b/PythonAPI/test/holoocean/sensors/test_sonar_sensor.py new file mode 100644 index 0000000000..940a3b37a9 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_sonar_sensor.py @@ -0,0 +1,79 @@ +import holoocean +import uuid +import os +import pytest +import numpy as np + + +@pytest.fixture +def config(): + c = { + "name": "test_location_sensor", + "world": "TestWorld", + "main_agent": "auv0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "auv0", + "agent_type": "HoveringAUV", + "sensors": [ + { + "sensor_type": "ImagingSonar", + "configuration": {"MinRange": 0.1, "MaxRange": 1}, + } + ], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], + } + return c + + +@pytest.mark.parametrize("size", [(0.02, 5.12), (0.1, 12.8), (0.05, 25.6)]) +def test_folder_creation(size, config): + """Make sure folders are made with the correct size""" + + config["octree_min"] = size[0] + config["octree_max"] = size[1] + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(10): + env.tick() + + dir = os.path.join( + holoocean.util.get_holoocean_path(), + "worlds/TestWorlds/LinuxNoEditor/Holodeck/Octrees", + ) + dir = os.path.join( + dir, f"{config['world']}/min{int(size[0]*100)}_max{int(size[1]*100)}" + ) + + assert os.path.isdir(dir), "Sonar folder wasn't created" + + +def test_blank(config): + """Test to make sure when we're in the middle of nowhere, we get nothing back""" + + config["agents"][0]["location"] = [-100, -100, -100] + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + for _ in range(10): + state = env.tick()["ImagingSonar"] + assert np.allclose(np.zeros_like(state), state) diff --git a/PythonAPI/test/holoocean/sensors/test_velocity_sensor.py b/PythonAPI/test/holoocean/sensors/test_velocity_sensor.py new file mode 100644 index 0000000000..c5a576eae8 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_velocity_sensor.py @@ -0,0 +1,64 @@ +import holoocean +import uuid + +uav_config = { + "name": "test_velocity_sensor", + "world": "TestWorld", + "main_agent": "uav0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "uav0", + "agent_type": "UavAgent", + "sensors": [ + { + "sensor_type": "VelocitySensor", + } + ], + "control_scheme": 0, + "location": [0, 0, 20], + } + ], +} + + +def test_velocity_sensor_uav_z_axis(): + """Drop the UAV, make sure the z velocity is increasingly negative as it falls. + Make sure it zeros out after it hits the ground, and then goes positive on takeoff + """ + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + + with holoocean.environments.HoloOceanEnvironment( + scenario=uav_config, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + last_z_velocity = env.tick()["VelocitySensor"][2] + + for _ in range(50): + new_z_velocity = env.tick()["VelocitySensor"][2] + assert new_z_velocity <= last_z_velocity, "The velocity didn't decrease!" + last_z_velocity = new_z_velocity + + # Make sure it hits the ground + for _ in range(60): + env.tick() + + last_z_velocity = env.tick()["VelocitySensor"][2] + + assert last_z_velocity <= 1e-4, "The velocity wasn't close enough to zero!" + + # Send it flying up into the air to make sure the z velocity increases + env.step([0, 0, 0, 100]) + + # z velocity should be positive now + for _ in range(20): + new_z_velocity = env.tick()["VelocitySensor"][2] + assert new_z_velocity >= last_z_velocity, "The velocity didn't increase!" + last_z_velocity = new_z_velocity + + +# TODO: Test other axises diff --git a/PythonAPI/test/holoocean/sensors/test_viewport_capture.py b/PythonAPI/test/holoocean/sensors/test_viewport_capture.py new file mode 100644 index 0000000000..d5b44dee07 --- /dev/null +++ b/PythonAPI/test/holoocean/sensors/test_viewport_capture.py @@ -0,0 +1,174 @@ +import copy +import uuid +import cv2 +import os +import time +import pytest + +from tests.utils.equality import mean_square_err + +import holoocean + +base_cfg = { + "name": "test_viewport_capture", + "world": "TestWorld", + "main_agent": "sphere0", + "frames_per_sec": False, + "agents": [ + { + "agent_name": "sphere0", + "agent_type": "SphereAgent", + "sensors": [{"sensor_type": "ViewportCapture"}], + "control_scheme": 0, + "location": [0.95, -1.75, 0.5], + } + ], +} + + +@pytest.mark.skipif( + "TestWorlds" not in holoocean.installed_packages(), + reason="Ocean package not installed", +) +def test_viewport_capture(resolution, request): + """Validates that the ViewportCapture camera is working at the expected resolutions + + Also incidentally validates that the viewport can be sized correctly + """ + + global base_cfg + + cfg = copy.deepcopy(base_cfg) + + cfg["window_width"] = resolution + cfg["window_height"] = resolution + + cfg["agents"][0]["sensors"][0]["configuration"] = { + "CaptureWidth": resolution, + "CaptureHeight": resolution, + } + + binary_path = holoocean.packagemanager.get_binary_path_for_package("TestWorlds") + with holoocean.environments.HoloOceanEnvironment( + scenario=cfg, + binary_path=binary_path, + show_viewport=False, + verbose=True, + uuid=str(uuid.uuid4()), + ) as env: + env.should_render_viewport(True) + + env.tick(5) + + pixels = env.tick()["ViewportCapture"][:, :, 0:3] + + assert pixels.shape == (resolution, resolution, 3) + filepath = str("/baseline_images/baseline_viewport_" + str(resolution) + ".png") + baseline = cv2.imread(str(request.fspath.dirname + filepath)) + # baseline = cv2.imread(os.path.join(request.fspath.dirname, "expected", "{}_viewport.png".format(resolution))) + + err = mean_square_err(pixels, baseline) + + assert ( + err < 1000 + ), "The expected screenshot did not match the actual screenshot!" + + +@pytest.mark.skipif( + "TestWorlds" not in holoocean.installed_packages(), + reason="Ocean package not installed", +) +def test_viewport_capture_after_teleport(env_1024, request): + """Validates that the ViewportCapture is updated after teleporting the camera + to a different location. + + Incidentally tests HoloOceanEnvironment.teleport_camera as well + """ + + # Other tests muck with this. Set it to true just in case + env_1024.should_render_viewport(True) + env_1024.move_viewport([0.9, -1.75, 0.5], [0, 0, 0]) + + for _ in range(5): + env_1024.tick() + + pixels = env_1024.tick()["ViewportCapture"][:, :, 0:3] + + # baseline = cv2.imread(os.path.join(request.fspath.dirname, "expected", "teleport_viewport_test.png")) + filepath = str("/baseline_images/baseline_viewport_moved_1024.png") + baseline = cv2.imread(str(request.fspath.dirname + filepath)) + err = mean_square_err(pixels, baseline) + + assert err < 1000, "The captured viewport differed from the expected screenshot!" + + +def validate_rendering_viewport_disabled(env_1024, between_tests_callback): + """Helper function for a few tests. Validates that rendering the viewport is actually disabled + by teleporting the agent and comparing the before/after RGBCamera captures. + + Args: + env_1024: environment to use + between_tests_callback: callback to call before teleporting the camera and taking a 2nd screenshot + + """ + start = time.perf_counter() + + for _ in range(10): + env_1024.tick() + elapsed_with_viewport = time.perf_counter() - start + + initial_screenshot = env_1024.tick()["ViewportCapture"][:, :, 0:3] + + between_tests_callback(env_1024) + + env_1024.move_viewport([781, 643, 4376], [381, 403, 3839]) + + start = time.perf_counter() + for _ in range(10): + env_1024.tick() + elapsed_without_viewport = time.perf_counter() - start + + final_screenshot = env_1024.tick()["ViewportCapture"][:, :, 0:3] + + err = mean_square_err(initial_screenshot, final_screenshot) + + assert err < 1000, "The screenshots were not identical after disabling rendering" + + # This is unreliable 👇 :/ + # assert elapsed_without_viewport < elapsed_with_viewport, \ + # "Holodeck did not tick faster without rendering the viewport. Was it actually " \ + # "disabled?" + + # cleanup environment + env_1024.should_render_viewport(True) + env_1024.tick() + + +def test_viewport_capture_stops_after_disabling_rendering(env_1024): + """Validates that the viewport render stops updating after disabling viewport + rendering. Check to make sure it renders faster, and that the images are + identical after teleporting the camera + """ + + def callback(env_1024): + env_1024.should_render_viewport(False) + + env_1024.should_render_viewport(True) + + validate_rendering_viewport_disabled(env_1024, callback) + + +def test_disabling_viewport_persists_after_reset(env_1024): + """Make sure that after disabling the viewport and calling .reset(), that rendering is still disabled""" + + def callback(env_1024): + pass + + env_1024.should_render_viewport(False) + + for _ in range(10): + env_1024.tick() + + env_1024.reset() + + validate_rendering_viewport_disabled(env_1024, callback) diff --git a/PythonAPI/test/holoocean/tox.ini b/PythonAPI/test/holoocean/tox.ini new file mode 100644 index 0000000000..2abb7256ac --- /dev/null +++ b/PythonAPI/test/holoocean/tox.ini @@ -0,0 +1,23 @@ +# content of: tox.ini , put in same dir as setup.py +[tox] +envlist = {py37,py38, py39, py310, py311}-{agents, lcm, scenarios, sensors} + +[testenv] +# install pytest in the virtualenv where commands will be executed +setenv = + HOLODECKPATH = packages +basepython = + py310: python3.10 +deps = + posix_ipc + pytest + numpy + opencv-python + scipy + lcm +commands = + # NOTE: you can run any command line tool here - not just tests + agents: pytest tests/agents + lcm: pytest tests/lcm + scenarios: pytest tests/scenarios + sensors: pytest tests/sensors \ No newline at end of file diff --git a/PythonAPI/test/holoocean/utils/__init__.py b/PythonAPI/test/holoocean/utils/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/PythonAPI/test/holoocean/utils/captures.py b/PythonAPI/test/holoocean/utils/captures.py new file mode 100644 index 0000000000..9c05d71fff --- /dev/null +++ b/PythonAPI/test/holoocean/utils/captures.py @@ -0,0 +1,86 @@ +import os +from typing import List, Tuple, Optional + +import cv2 + +from tests.utils.equality import mean_square_err + + +def display_multiple(images: List[Tuple[List, Optional[str]]]): + """Displays one or more captures in a CV2 window. Useful for debugging + + Args: + images: List of tuples containing MxNx3 pixel arrays and optional titles OR + list of image data + """ + for image in images: + if isinstance(image, tuple): + image_data = image[0] + else: + image_data = image + + if isinstance(image, tuple) and len(image) > 1: + title = image[1] + else: + title = "Camera Output" + + cv2.namedWindow(title) + cv2.moveWindow(title, 500, 500) + cv2.imshow(title, image_data) + cv2.waitKey(0) + cv2.destroyAllWindows() + + +def display(pixels, title="Camera Output"): + """Displays the given capture in a CV2 window. Useful for debugging + + Args: + pixels: MxNx3 array of pixels + title: The title of the window + + """ + display_multiple([(pixels, title)]) + + +def compare_rgb_sensor_data_with_baseline( + sensor_data, base_path, baseline_name, show_images=False +) -> float: + """ + Compare data from RGB sensor with baseline file in `expected` folder and + return mean squared error + """ + pixels = sensor_data[:, :, 0:3] + baseline = cv2.imread( + os.path.join(base_path, "expected", "{}.png".format(baseline_name)) + ) + if show_images: + # Show images when debugging--this will block tests until user input + # is provided + display_multiple([(pixels, "pixels"), (baseline, "baseline")]) + return mean_square_err(pixels, baseline) + + +def compare_rgb_sensor_data(sensor_data_1, sensor_data_2, show_images=False) -> float: + """ + Compare data from RGB sensors + return mean squared error + """ + pixels_1 = sensor_data_1[:, :, 0:3] + pixels_2 = sensor_data_2[:, :, 0:3] + if show_images: + # Show images when debugging--this will block tests until user input + # is provided + display_multiple([(pixels_1, "image 1"), (pixels_2, "image 2")]) + return mean_square_err(pixels_1, pixels_2) + + +def write_image_from_rgb_sensor_data(sensor_data, base_path, name): + """For use in saving `expected` images from RGB camera sensor data + + Args: + sensor_data: Data from RGB sensor tick + base_path: Path to test directory containing `expected` directory + name: Name of file to save, not including `png` file extension + """ + pixels = sensor_data[:, :, 0:3] + cv2.imwrite(os.path.join(base_path, "expected", "{}.png".format(name)), pixels) diff --git a/PythonAPI/test/holoocean/utils/equality.py b/PythonAPI/test/holoocean/utils/equality.py new file mode 100644 index 0000000000..0ce60ab652 --- /dev/null +++ b/PythonAPI/test/holoocean/utils/equality.py @@ -0,0 +1,26 @@ +import numpy as np + + +def almost_equal(item1, item2, r_thresh=0.01, a_thresh=1e-4): + item1 = np.array(item1).flatten() + item2 = np.array(item2).flatten() + if len(item1) != len(item2): + return False + + return all(np.isclose(item1, item2, rtol=r_thresh, atol=a_thresh)) + + +def mean_square_err(im1, im2): + """Compute the mean square error of the two images to determine if they + are equivalent. + + Args: + im1 (np array): + im2 (np array): + + Returns: integer, lower is more similar + + """ + err = np.sum((im1.astype("float") - im2.astype("float")) ** 2) + err /= float(im1.shape[0] * im1.shape[1]) + return err diff --git a/Unreal/CarlaUE4/Plugins/Carla/Carla.uplugin b/Unreal/CarlaUE4/Plugins/Carla/Carla.uplugin index 88c94be10f..5beaa44e0e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Carla.uplugin +++ b/Unreal/CarlaUE4/Plugins/Carla/Carla.uplugin @@ -1,7 +1,7 @@ { "FileVersion": 3, "Version": 1, - "VersionName": "2.10.0", + "VersionName": "2.10.1", "FriendlyName": "CARLA", "Description": "Open-source simulator for autonomous driving research.", "Category": "Science", diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin b/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin new file mode 100644 index 0000000000..e5ca97b4af --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/HoloOcean.uplugin @@ -0,0 +1,24 @@ +{ + "FileVersion": 1, + "Version": 1, + "VersionName": "0.1-alpha", + "FriendlyName": "HoloOcean", + "Description": "HoloOcean", + "Category": "Simulation", + "CreatedBy": "OpoenHUTB", + "CreatedByURL": "https://github.com/OpenHUTB", + "DocsURL": "https://openhutb.github.io/mujoco_plugin/underwater/", + "MarketplaceURL": "", + "SupportURL": "https://github.com/OpenHUTB/hutb/issues", + "CanContainContent": true, + "IsBetaVersion": false, + "IsExperimentalVersion": true, + "Installed": true, + "Modules": [ + { + "Name": "Holodeck", + "Type": "Editor", + "LoadingPhase": "Default" + } + ] +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Android.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Android.cpp new file mode 100644 index 0000000000..4fb893dac0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Android.cpp @@ -0,0 +1,201 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Android.h" +#include "Holodeck.h" + +AAndroid::AAndroid() { + PrimaryActorTick.bCanEverTick = true; + bCollisionsAreVisible = false; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.AndroidController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; +} + +void AAndroid::InitializeAgent() { + Super::InitializeAgent(); + SkeletalMesh = Cast(RootComponent); +} + +void AAndroid::Tick(float DeltaTime) { + Super::Tick(DeltaTime); + ApplyTorques(); +} + +void AAndroid::SetCollisionsVisible(bool Visible) { + bCollisionsAreVisible = Visible; +} + +bool AAndroid::GetCollisionsVisible() { + return bCollisionsAreVisible; +} + +void AAndroid::ApplyTorques() { + UE_LOG(LogHolodeck, Verbose, TEXT("AAndroid::ApplyTorques")); + int ComInd = 0; + + for (int JointInd = 0; JointInd < NUM_JOINTS; JointInd++) { + + FName JointName = Joints[JointInd]; + + // Get rotation of that socket + FQuat RotQuat = SkeletalMesh->GetSocketQuaternion(JointName); + FVector RotationVector = FVector(0.0f, 0.0f, 0.0f); + + // Apply Swing 1 Torque if non zero + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.Z = RotForce; + } + ComInd++; + + // Apply Swing 2 if Torque non zero and is 2 or 3 axis joint + if (JointInd < (NUM_2_PLUS_3_AXIS_JOINTS)) { + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.Y = RotForce; + } + ComInd++; + + // Apply Twist if Torque non zero and is 3 axis joint + if (JointInd < NUM_3_AXIS_JOINTS) { + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.X = RotForce; + } + ComInd++; + } + } + // Convert torque from m/rhs to cm/lhs + RotationVector = ConvertTorque(RotationVector, ClientToUE); + SkeletalMesh->AddTorqueInRadians(RotQuat.RotateVector(RotationVector), JointName, false); + } +} + +const FName AAndroid::Joints[] = { + + // Head, Spine, and Arm joints. Each has [swing1, swing2, twist] + FName(TEXT("head")), + FName(TEXT("neck_01")), + FName(TEXT("spine_02")), + FName(TEXT("spine_01")), + FName(TEXT("upperarm_l")), + FName(TEXT("lowerarm_l")), + FName(TEXT("hand_l")), + FName(TEXT("upperarm_r")), + FName(TEXT("lowerarm_r")), + FName(TEXT("hand_r")), + + // Leg Joints. Each has [swing1, swing2, twist] + FName(TEXT("thigh_l")), + FName(TEXT("calf_l")), + FName(TEXT("foot_l")), + FName(TEXT("ball_l")), + FName(TEXT("thigh_r")), + FName(TEXT("calf_r")), + FName(TEXT("foot_r")), + FName(TEXT("ball_r")), + + // First joint of each finger. Has only [swing1, swing2] + FName(TEXT("thumb_01_l")), + FName(TEXT("index_01_l")), + FName(TEXT("middle_01_l")), + FName(TEXT("ring_01_l")), + FName(TEXT("pinky_01_l")), + FName(TEXT("thumb_01_r")), + FName(TEXT("index_01_r")), + FName(TEXT("middle_01_r")), + FName(TEXT("ring_01_r")), + FName(TEXT("pinky_01_r")), + + // Second joint of each finger. Has only [swing1] + FName(TEXT("thumb_02_l")), + FName(TEXT("index_02_l")), + FName(TEXT("middle_02_l")), + FName(TEXT("ring_02_l")), + FName(TEXT("pinky_02_l")), + FName(TEXT("thumb_02_r")), + FName(TEXT("index_02_r")), + FName(TEXT("middle_02_r")), + FName(TEXT("ring_02_r")), + FName(TEXT("pinky_02_r")), + + // Third joint of each finger. Has only [swing1] + FName(TEXT("thumb_03_l")), + FName(TEXT("index_03_l")), + FName(TEXT("middle_03_l")), + FName(TEXT("ring_03_l")), + FName(TEXT("pinky_03_l")), + FName(TEXT("thumb_03_r")), + FName(TEXT("index_03_r")), + FName(TEXT("middle_03_r")), + FName(TEXT("ring_03_r")), + FName(TEXT("pinky_03_r")), +}; + +// Don't forget to update AAndroid::NumBones after changing this array! +const FName AAndroid::BoneNames[] = { + FName(TEXT("pelvis")), + FName(TEXT("spine_01")), + FName(TEXT("spine_02")), + FName(TEXT("spine_03")), + FName(TEXT("clavicle_l")), + FName(TEXT("upperarm_l")), + FName(TEXT("lowerarm_l")), + FName(TEXT("hand_l")), + FName(TEXT("index_01_l")), + FName(TEXT("index_02_l")), + FName(TEXT("index_03_l")), + FName(TEXT("middle_01_l")), + FName(TEXT("middle_02_l")), + FName(TEXT("middle_03_l")), + FName(TEXT("pinky_01_l")), + FName(TEXT("pinky_02_l")), + FName(TEXT("pinky_03_l")), + FName(TEXT("ring_01_l")), + FName(TEXT("ring_02_l")), + FName(TEXT("ring_03_l")), + FName(TEXT("thumb_01_l")), + FName(TEXT("thumb_02_l")), + FName(TEXT("thumb_03_l")), + FName(TEXT("lowerarm_twist_01_l")), + FName(TEXT("upperarm_twist_01_l")), + FName(TEXT("clavicle_r")), + FName(TEXT("upperarm_r")), + FName(TEXT("lowerarm_r")), + FName(TEXT("hand_r")), + FName(TEXT("index_01_r")), + FName(TEXT("index_02_r")), + FName(TEXT("index_03_r")), + FName(TEXT("middle_01_r")), + FName(TEXT("middle_02_r")), + FName(TEXT("middle_03_r")), + FName(TEXT("pinky_01_r")), + FName(TEXT("pinky_02_r")), + FName(TEXT("pinky_03_r")), + FName(TEXT("ring_01_r")), + FName(TEXT("ring_02_r")), + FName(TEXT("ring_03_r")), + FName(TEXT("thumb_01_r")), + FName(TEXT("thumb_02_r")), + FName(TEXT("thumb_03_r")), + FName(TEXT("lowerarm_twist_01_r")), + FName(TEXT("upperarm_twist_01_r")), + FName(TEXT("neck_01")), + FName(TEXT("head")), + FName(TEXT("thigh_l")), + FName(TEXT("calf_l")), + FName(TEXT("calf_twist_01_l")), + FName(TEXT("foot_l")), + FName(TEXT("ball_l")), + FName(TEXT("thigh_twist_01_l")), + FName(TEXT("thigh_r")), + FName(TEXT("calf_r")), + FName(TEXT("calf_twist_01_r")), + FName(TEXT("foot_r")), + FName(TEXT("ball_r")), + FName(TEXT("thigh_twist_01_r")) +}; + +// If you change this number, change the corresponding number in RelativeSkeletalPositionSensor.__init__ +const int AAndroid::NumBones = 60; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidControlSchemeMaxTorque.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidControlSchemeMaxTorque.cpp new file mode 100644 index 0000000000..0ffe0fb0c7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidControlSchemeMaxTorque.cpp @@ -0,0 +1,60 @@ +#include "Holodeck.h" +#include "AndroidControlSchemeMaxTorque.h" + + + + +UAndroidControlSchemeMaxTorque::UAndroidControlSchemeMaxTorque(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer) {} + +void UAndroidControlSchemeMaxTorque::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + float* InputCommandFloat = static_cast(InputCommand); + float* CommandArrayFloat = static_cast(CommandArray); + + if (Android == nullptr) { + Android = static_cast(AndroidController->GetPawn()); + if (Android == nullptr) { + UE_LOG(LogHolodeck, Error, TEXT("UAndroidControlSchemeMaxTorque couldn't get Android reference")); + return; + } + } + + USkeletalMeshComponent* SkeletalMesh = Android->SkeletalMesh; + + int ComInd = 0; + + for (int BoneInd = 0; BoneInd < Android->NUM_JOINTS; BoneInd++) { + + // Joint names directly correspond to bone names + FName BoneName = Android->Joints[BoneInd]; + + // Get the mass of the bone connected to this joint + float BoneMass = SkeletalMesh->GetBoneMass(BoneName, true); + + // Set the Scalar depending on whether its a finger joint or large muscle joint + float scalar = TorqueScalarFingers; + if (BoneInd < Android->NUM_3_AXIS_JOINTS){ + scalar = TorqueScalarMuscles; + } + + CommandArrayFloat[ComInd] = calcTorqueToApply(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + + // Apply Swing 2 if Torque non is 2 or 3 axis joint + if (BoneInd < (Android->NUM_2_PLUS_3_AXIS_JOINTS)) { + CommandArrayFloat[ComInd] = calcTorqueToApply(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + + // Apply Twist if Torque is 3 axis joint + if (BoneInd < Android->NUM_3_AXIS_JOINTS) { + CommandArrayFloat[ComInd] = calcTorqueToApply(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + } + } + } +} + +float UAndroidControlSchemeMaxTorque::calcTorqueToApply(float CommandValue, float BoneMass, float TorqueScalar){ + CommandValue = FMath::Clamp(CommandValue, MinCommand, MaxCommand); + return CommandValue * ( BoneMass * TorqueScalar ); // See function declaration for explaination of equation +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidController.cpp new file mode 100644 index 0000000000..6ece3f4fbf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/AndroidController.cpp @@ -0,0 +1,46 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "AndroidController.h" + +AAndroidController::AAndroidController(const FObjectInitializer& ObjectInitializer) : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogHolodeck, Log, TEXT("Android Controller Initialized")); +} + +AAndroidController::~AAndroidController() {} + +void AAndroidController::OnPossess(APawn* PawnParam) { + Super::OnPossess(PawnParam); + UE_LOG(LogHolodeck, Log, TEXT("Android Controller possessing pawn")); + + TArray Components; + PawnParam->GetComponents(Components); + + if (Components.Num() < 1) { + UE_LOG(LogHolodeck, Error, TEXT("Couldn't find USkeletelMeshComponent for PawnParam")); + } else { + SkeletalMeshComponent = Components[0]; + } + + this->ControlScheme->SetSkeletalMesh(this->SkeletalMeshComponent, const_cast(AAndroid::Joints)); + + ActionBufferFloatPtr = static_cast(ActionBuffer); +} + +void AAndroidController::AddControlSchemes() { + + ControlScheme = NewObject(); + ControlScheme->SetController(this); + ControlScheme->SetControlSchemeSizeInBytes(AAndroid::TOTAL_DOF); + + ControlScheme->SetJointSizes( + AAndroid::NUM_3_AXIS_JOINTS, + AAndroid::NUM_2_AXIS_JOINTS, + AAndroid::NUM_1_AXIS_JOINTS + ); + + ControlScheme->SetFingerStartIndex(AAndroid::NUM_3_AXIS_JOINTS * 3 - 1); + + ControlSchemes.Add(ControlScheme); + +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgent.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgent.cpp new file mode 100644 index 0000000000..b08d92b775 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgent.cpp @@ -0,0 +1,129 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HandAgent.h" + +AHandAgent::AHandAgent() { + PrimaryActorTick.bCanEverTick = true; + + // Set the default controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.HandAgentController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; +} + +void AHandAgent::InitializeAgent() { + Super::InitializeAgent(); + SkeletalMesh = Cast(RootComponent); + memset(this->CommandArray, 0, sizeof this->CommandArray); +} + +void AHandAgent::Tick(float DeltaTime) { + Super::Tick(DeltaTime); + ApplyTorques(); + ApplyLevitation(); +} + +void AHandAgent::ApplyTorques() { + UE_LOG(LogHolodeck, Verbose, TEXT("AHandAgent::ApplyTorques")); + int ComInd = 0; + + for (int JointInd = 0; JointInd < NUM_JOINTS; JointInd++) { + + FName JointName = Joints[JointInd]; + + // Get rotation of that socket + FQuat RotQuat = SkeletalMesh->GetSocketQuaternion(JointName); + FVector RotationVector = FVector(0.0f, 0.0f, 0.0f); + + // Apply Swing 1 Torque if non zero + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.Z = RotForce; + } + ComInd++; + + // Apply Swing 2 if Torque non zero and is 2 or 3 axis joint + if (JointInd < (NUM_2_PLUS_3_AXIS_JOINTS)) { + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.Y = RotForce; + } + ComInd++; + + // Apply Twist if Torque non zero and is 3 axis joint + if (JointInd < NUM_3_AXIS_JOINTS) { + if (CommandArray[ComInd] != 0) { + float RotForce = FMath::Clamp(CommandArray[ComInd], -MAX_TORQUE, MAX_TORQUE); + RotationVector.X = RotForce; + } + ComInd++; + } + } + // Convert torque from m/rhs to cm/lhs + RotationVector = ConvertTorque(RotationVector, ClientToUE); + SkeletalMesh->AddTorqueInRadians(RotQuat.RotateVector(RotationVector), JointName, false); + } +} + +void AHandAgent::ApplyLevitation() { + float deltaX = std::min(CommandArray[AHandAgent::TOTAL_JOINT_DOF], MAX_MOVEMENT_METERS); + float deltaY = std::min(CommandArray[AHandAgent::TOTAL_JOINT_DOF + 1], MAX_MOVEMENT_METERS); + float deltaZ = std::min(CommandArray[AHandAgent::TOTAL_JOINT_DOF + 2], MAX_MOVEMENT_METERS); + + FVector DeltaLocation = FVector(deltaX, deltaY, deltaZ); + + DeltaLocation = ConvertLinearVector(DeltaLocation, ClientToUE); + + AddActorWorldOffset(DeltaLocation, true, NULL, ETeleportType::None); +} + +const FName AHandAgent::Joints[] = { + + // Head, Spine, and Arm joints. Each has [swing1, swing2, twist] + FName(TEXT("hand_r")), + + // First joint of each finger. Has only [swing1, swing2] + FName(TEXT("thumb_01_r")), + FName(TEXT("index_01_r")), + FName(TEXT("middle_01_r")), + FName(TEXT("ring_01_r")), + FName(TEXT("pinky_01_r")), + + // Second joint of each finger. Has only [swing1] + FName(TEXT("thumb_02_r")), + FName(TEXT("index_02_r")), + FName(TEXT("middle_02_r")), + FName(TEXT("ring_02_r")), + FName(TEXT("pinky_02_r")), + + // Third joint of each finger. Has only [swing1] + FName(TEXT("thumb_03_r")), + FName(TEXT("index_03_r")), + FName(TEXT("middle_03_r")), + FName(TEXT("ring_03_r")), + FName(TEXT("pinky_03_r")), +}; + +// Don't forget to update AHandAgent::NumBones after changing this array! +const FName AHandAgent::BoneNames[] = { + FName(TEXT("lowerarm_r")), + FName(TEXT("hand_r")), + FName(TEXT("index_01_r")), + FName(TEXT("index_02_r")), + FName(TEXT("index_03_r")), + FName(TEXT("middle_01_r")), + FName(TEXT("middle_02_r")), + FName(TEXT("middle_03_r")), + FName(TEXT("pinky_01_r")), + FName(TEXT("pinky_02_r")), + FName(TEXT("pinky_03_r")), + FName(TEXT("ring_01_r")), + FName(TEXT("ring_02_r")), + FName(TEXT("ring_03_r")), + FName(TEXT("thumb_01_r")), + FName(TEXT("thumb_02_r")), + FName(TEXT("thumb_03_r")) +}; + +// If you change this number, change the corresponding number in RelativeSkeletalPositionSensor.__init__ +const int AHandAgent::NumBones = 17; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentController.cpp new file mode 100644 index 0000000000..6671f5b89b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentController.cpp @@ -0,0 +1,47 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HandAgentController.h" + +AHandAgentController::AHandAgentController(const FObjectInitializer& ObjectInitializer) : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogHolodeck, Log, TEXT("Hand Agent Controller Initialized")); +} + +void AHandAgentController::AddControlSchemes() { + this->JointTorqueControlScheme = NewObject(); + this->JointTorqueControlScheme->SetController(this); + this->JointTorqueControlScheme->SetControlSchemeSizeInBytes(AHandAgent::TOTAL_JOINT_DOF); + + this->JointTorqueControlScheme->SetJointSizes( + AHandAgent::NUM_3_AXIS_JOINTS, + AHandAgent::NUM_2_AXIS_JOINTS, + AHandAgent::NUM_1_AXIS_JOINTS); + + this->JointTorqueControlScheme->SetFingerStartIndex(AHandAgent::NUM_3_AXIS_JOINTS * 3 - 1); + + // Create the floating control scheme too + this->HandAgentFloatControlScheme = NewObject(); + this->HandAgentFloatControlScheme->SetTorqueControlScheme(this->JointTorqueControlScheme); + + ControlSchemes.Add(this->JointTorqueControlScheme); + ControlSchemes.Add(this->HandAgentFloatControlScheme); + +} + +void AHandAgentController::OnPossess(APawn* PawnParam) { + Super::OnPossess(PawnParam); + UE_LOG(LogHolodeck, Log, TEXT("HandAgent Controller possessing pawn")); + + TArray Components; + PawnParam->GetComponents(Components); + + if (Components.Num() < 1) { + UE_LOG(LogHolodeck, Fatal, TEXT("Couldn't find USkeletelMeshComponent for PawnParam")); + } + + SkeletalMeshComponent = Components[0]; + + this->JointTorqueControlScheme->SetSkeletalMesh(SkeletalMeshComponent, const_cast(AHandAgent::Joints)); + + ActionBufferFloatPtr = static_cast(ActionBuffer); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentMaxTorqueFloat.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentMaxTorqueFloat.cpp new file mode 100644 index 0000000000..f711131fe1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HandAgentMaxTorqueFloat.cpp @@ -0,0 +1,28 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HandAgentMaxTorqueFloat.h" + +void UHandAgentMaxTorqueFloat::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + + float* InputCommandFloat = static_cast(InputCommand); + + // Call the other controller so it can do its thing + this->TorqueControlScheme->Execute(CommandArray, InputCommand, DeltaSeconds); + + // Copy out the floating directions + // Floating around is handled by the HandAgent itself + for (int i = 0; i < 3; ++i) { + static_cast(CommandArray)[AHandAgent::TOTAL_JOINT_DOF + i] = + InputCommandFloat[AHandAgent::TOTAL_JOINT_DOF + i]; + } + +} + +void UHandAgentMaxTorqueFloat::SetTorqueControlScheme(UJointMaxTorqueControlScheme* Scheme) { + this->TorqueControlScheme = Scheme; +} + +unsigned int UHandAgentMaxTorqueFloat::GetControlSchemeSizeInBytes() const { + return this->TorqueControlScheme->GetControlSchemeSizeInBytes() + 3; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUV.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUV.cpp new file mode 100644 index 0000000000..8f3d850b44 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUV.cpp @@ -0,0 +1,83 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "HoveringAUV.h" + +const float AUV_MAX_FORCE = 100; + +// Sets default values +AHoveringAUV::AHoveringAUV() { + PrimaryActorTick.bCanEverTick = true; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.HoveringAUVController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; + + // This values are all pulled from the solidworks file + this->Volume = .03554577; + this->CenterBuoyancy = FVector(-5.96, 0.29, -1.85); + this->CenterMass = FVector(-5.9, 0.46, -2.82); + this->MassInKG = 31.02; + this->OffsetToOrigin = FVector(-0.7, -2, 32); +} + +// Sets all values that we need +void AHoveringAUV::InitializeAgent() { + RootMesh = Cast(RootComponent); + + if(Perfect){ + this->CenterMass = (thrusterLocations[0] + thrusterLocations[2]) / 2; + this->CenterMass.Z = thrusterLocations[7].Z; + + this->CenterBuoyancy = CenterMass; + this->CenterBuoyancy.Z += 5; + + this->Volume = MassInKG / WaterDensity; + } + + // Apply OffsetToOrigin to all of our custom position vectors + for(int i=0;i<8;i++){ + thrusterLocations[i] += OffsetToOrigin; + } + + Super::InitializeAgent(); +} + +// Called every frame +void AHoveringAUV::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + ApplyBuoyantForce(); + ApplyThrusters(); +} + +void AHoveringAUV::ApplyThrusters(){ + //Get all the values we need once + FVector ActorLocation = GetActorLocation(); + FRotator ActorRotation = GetActorRotation(); + + // Iterate through vertical thrusters + for(int i=0;i<4;i++){ + float force = FMath::Clamp(CommandArray[i], -AUV_MAX_FORCE, AUV_MAX_FORCE); + + FVector LocalForce = FVector(0, 0, force); + LocalForce = ConvertLinearVector(LocalForce, ClientToUE); + + RootMesh->AddForceAtLocationLocal(LocalForce, thrusterLocations[i]); + } + + // Iterate through angled thrusters + for(int i=4;i<8;i++){ + float force = FMath::Clamp(CommandArray[i], -AUV_MAX_FORCE, AUV_MAX_FORCE); + + // 4 + 6 have negative y + FVector LocalForce = FVector(0, 0, 0); + if(i % 2 == 0) + LocalForce = FVector(force/FMath::Sqrt(2), force/FMath::Sqrt(2), 0); + else + LocalForce = FVector(force/FMath::Sqrt(2), -force/FMath::Sqrt(2), 0); + LocalForce = ConvertLinearVector(LocalForce, ClientToUE); + + RootMesh->AddForceAtLocationLocal(LocalForce, thrusterLocations[i]); + } + +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUVController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUVController.cpp new file mode 100644 index 0000000000..276f507c5e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/HoveringAUVController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "HoveringAUVController.h" + +AHoveringAUVController::AHoveringAUVController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogTemp, Warning, TEXT("HoveringAUV Controller Initialized")); +} + +AHoveringAUVController::~AHoveringAUVController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/JointMaxTorqueControlScheme.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/JointMaxTorqueControlScheme.cpp new file mode 100644 index 0000000000..1f67374453 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/JointMaxTorqueControlScheme.cpp @@ -0,0 +1,74 @@ +#include "Holodeck.h" +#include "JointMaxTorqueControlScheme.h" + + +UJointMaxTorqueControlScheme::UJointMaxTorqueControlScheme(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer) {} + +unsigned int UJointMaxTorqueControlScheme::GetControlSchemeSizeInBytes() const { + return this->ControlSchemeSizeBytes; +} + +void UJointMaxTorqueControlScheme::SetControlSchemeSizeInBytes(int size) { + this->ControlSchemeSizeBytes = size; +} + +void UJointMaxTorqueControlScheme::SetSkeletalMesh(USkeletalMeshComponent* Skeletal, FName Joints[]) { + this->SkeletalMesh = Skeletal; + this->JointNames = Joints; +} + +void UJointMaxTorqueControlScheme::SetJointSizes(int ThreeAxis, int TwoAxis, int OneAxis) { + this->ThreeAxisJoints = ThreeAxis; + this->TwoAxisJoints = TwoAxis; + this->OneAxisJoints = OneAxis; +} + +void UJointMaxTorqueControlScheme::SetFingerStartIndex(int size) { + this->FingerStartIndex = size; +} + +void UJointMaxTorqueControlScheme::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + float* InputCommandFloat = static_cast(InputCommand); + float* CommandArrayFloat = static_cast(CommandArray); + + int ComInd = 0; + + unsigned TotalJoints = this->ThreeAxisJoints + this->TwoAxisJoints + this->OneAxisJoints; + + for (unsigned BoneInd = 0; BoneInd < TotalJoints; BoneInd++) { + + // Joint names directly correspond to bone names + FName BoneName = this->JointNames[BoneInd]; + + // Get the mass of the bone connected to this joint + float BoneMass = SkeletalMesh->GetBoneMass(BoneName, true); + + // Set the Scalar depending on whether its a finger joint or large muscle joint + float scalar = TorqueScalarFingers; + + if (BoneInd < this->FingerStartIndex) { + scalar = TorqueScalarMuscles; + } + + CommandArrayFloat[ComInd] = this->CalculateTorque(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + + // Apply Swing 2 if Torque non is 2 or 3 axis joint + if (BoneInd < (this->ThreeAxisJoints + this->TwoAxisJoints)) { + CommandArrayFloat[ComInd] = this->CalculateTorque(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + + // Apply Twist if Torque is 3 axis joint + if (BoneInd < this->ThreeAxisJoints) { + CommandArrayFloat[ComInd] = this->CalculateTorque(InputCommandFloat[ComInd], BoneMass, scalar); + ComInd++; + } + } + } +} + +float UJointMaxTorqueControlScheme::CalculateTorque(float CommandValue, float BoneMass, float TorqueScalar) { + CommandValue = FMath::Clamp(CommandValue, MinCommand, MaxCommand); + return CommandValue * (BoneMass * TorqueScalar); // See function declaration for explanation of equation +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgent.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgent.cpp new file mode 100644 index 0000000000..fa96465c33 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgent.cpp @@ -0,0 +1,32 @@ +#include "Holodeck.h" +#include "NavAgent.h" + + +// Sets default values +ANavAgent::ANavAgent() { + PrimaryActorTick.bCanEverTick = true; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.NavAgentController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; +} + +void ANavAgent::InitializeAgent() { + Super::InitializeAgent(); + Target = this->GetActorLocation(); +} + +// Called every frame +void ANavAgent::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + Target = FVector(CommandArray[0], CommandArray[1], CommandArray[2]); + Target = ConvertLinearVector(Target, ClientToUE); +} + +FVector ANavAgent::GetTarget() { + return Target; +} + +void ANavAgent::SetTarget(float x, float y, float z) { + Target = FVector(x, y, z); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgentController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgentController.cpp new file mode 100644 index 0000000000..2136ad93f0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/NavAgentController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "NavAgentController.h" + +ANavAgentController::ANavAgentController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogHolodeck, Log, TEXT("NavAgent Controller Initialized")); +} + +ANavAgentController::~ANavAgentController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/RawControlScheme.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/RawControlScheme.cpp new file mode 100644 index 0000000000..59f8040894 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/RawControlScheme.cpp @@ -0,0 +1,17 @@ +#include "Holodeck.h" +#include "RawControlScheme.h" + + +URawControlScheme::URawControlScheme(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer) {} + +URawControlScheme::URawControlScheme(AHolodeckAgentInterface* const ControlledAgent) { + Agent = ControlledAgent; + if (Agent == nullptr) { + UE_LOG(LogHolodeck, Fatal, TEXT("Agent couldn't be set in control scheme!")); + } +} + +void URawControlScheme::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + memcpy(CommandArray, InputCommand, Agent->GetRawActionSizeInBytes()); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobot.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobot.cpp new file mode 100644 index 0000000000..923df69e1a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobot.cpp @@ -0,0 +1,35 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "SphereRobot.h" + + +// Sets default values +ASphereRobot::ASphereRobot() { + PrimaryActorTick.bCanEverTick = true; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.SphereRobotController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; +} + +void ASphereRobot::InitializeAgent() { + Super::InitializeAgent(); +} + +// Called every frame +void ASphereRobot::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + ForwardSpeed = FMath::Clamp(CommandArray[0], -MAX_FORWARD_SPEED, MAX_FORWARD_SPEED); + RotSpeed = FMath::Clamp(CommandArray[1], -MAX_ROTATION_SPEED, MAX_ROTATION_SPEED); + + FVector DeltaLocation = GetActorForwardVector() * ForwardSpeed; + DeltaLocation = ConvertLinearVector(DeltaLocation, ClientToUE); + + AddActorWorldOffset(DeltaLocation, true); + FRotator DeltaRotation(0, RotSpeed, 0); + + DeltaRotation = ConvertAngularVector(DeltaRotation, ClientToUE); + + AddActorWorldRotation(DeltaRotation); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobotController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobotController.cpp new file mode 100644 index 0000000000..1c2c43bc44 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/SphereRobotController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "SphereRobotController.h" + +ASphereRobotController::ASphereRobotController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogTemp, Warning, TEXT("SphereRobot Controller Initialized")); +} + +ASphereRobotController::~ASphereRobotController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUV.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUV.cpp new file mode 100644 index 0000000000..95649146b3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUV.cpp @@ -0,0 +1,97 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "TorpedoAUV.h" + +// Sets default values +ATorpedoAUV::ATorpedoAUV() { + PrimaryActorTick.bCanEverTick = true; + + // Set the default controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.TorpedoAUVController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; + + this->OffsetToOrigin = FVector(0, 0, 0); + this->CenterBuoyancy = FVector(0,0,7); + this->CenterMass = FVector(0,0,0); + this->MassInKG = 36; + this->Volume = MassInKG / WaterDensity; //0.0342867409204; +} + +void ATorpedoAUV::InitializeAgent() { + RootMesh = Cast(RootComponent); + + Super::InitializeAgent(); +} + +// Called every frame +void ATorpedoAUV::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + ApplyBuoyantForce(); + + // Apply propeller + float ThrustToApply = FMath::Clamp(CommandArray[4], TAUV_MIN_THRUST, TAUV_MAX_THRUST); + FVector LocalThrust = FVector(ThrustToApply, 0, 0); + LocalThrust = ConvertLinearVector(LocalThrust, ClientToUE); + RootMesh->AddForceAtLocationLocal(LocalThrust, thruster); + + // Apply fin forces + for(int i=0;i<4;i++){ + ApplyFin(i); + } +} + +/** Based on the models found in + * Preliminary Evaluation of Cooperative Navigation of Underwater Vehicles + * without a DVL Utilizing a Dynamic Process Model, Section III-3) Control Inputs + * https://ieeexplore.ieee.org/document/8460970 +*/ +void ATorpedoAUV::ApplyFin(int i){ + // Get rotations + float commandAngle = FMath::Clamp(CommandArray[i], TAUV_MIN_FIN, TAUV_MAX_FIN); + FRotator bodyToWorld = this->GetActorRotation(); + FRotator finToBody = UKismetMathLibrary::ComposeRotators(FRotator(commandAngle, 0, 0), finRotation[i]); + + // get velocity at fin location, in fin frame + FVector velWorld = RootMesh->GetPhysicsLinearVelocityAtPoint(finTranslation[i]); + FVector velBody = bodyToWorld.UnrotateVector(velWorld); + FVector velFin = finToBody.UnrotateVector(velBody); + + // get flow angle and flow frame + double angle = UKismetMathLibrary::DegAtan2(-velFin.Z, velFin.X); + while(angle-commandAngle > 90){ + angle -= 180; + } + while(angle-commandAngle < -90){ + angle += 180; + } + FRotator WToBody = UKismetMathLibrary::ComposeRotators(FRotator(angle, 0, 0), finRotation[i]); + + // Calculate force in flow frame + double u2 = velFin.Z*velFin.Z + velFin.X*velFin.X; + // TODO: Verify these coefficients + // I've just adjusted these until they seem to behave correctly + double sin = -FMath::Sin(angle*3.14/180); + double drag = 0.5 * u2 * sin*sin / 400; + double lift = 0.5 * u2 * sin*sin*sin / 10; + // Sometimes they get out of hand, clamp them + FVector fW = -FVector(drag, 0, lift); + fW = fW.GetClampedToMaxSize(300); + // flip it if we're going backwards + if(velBody.X < 0){ + fW *= -1; + } + + // Move force into body frame & apply + FVector fBody = WToBody.RotateVector(fW); + RootMesh->AddForceAtLocationLocal(fBody, finTranslation[i]); + + // // Used to draw forces/frames for debugging + // UE_LOG(LogHolodeck, Warning, TEXT("Angle: %f, drag %f, lift %f"), angle, fW.X, fW.Z); + // FTransform finCoord = FTransform(finToBody, finTranslation[i]) * GetActorTransform(); + // FVector fWorld = bodyToWorld.RotateVector(fBody); + // // View force vector + // DrawDebugLine(GetWorld(), finCoord.GetTranslation(), finCoord.GetTranslation()+fWorld*10, FColor::Red, false, .1, ECC_WorldStatic, 1.f); + // // View angle of attack coordinate frame + // DrawDebugCoordinateSystem(GetWorld(), finCoord.GetTranslation(), finCoord.Rotator(), 15, false, .1, ECC_WorldStatic, 1.f); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUVController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUVController.cpp new file mode 100644 index 0000000000..ff3a1edd94 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TorpedoAUVController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "TorpedoAUVController.h" + +ATorpedoAUVController::ATorpedoAUVController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogTemp, Warning, TEXT("TorpedoAUV Controller Initialized")); +} + +ATorpedoAUVController::~ATorpedoAUVController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgent.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgent.cpp new file mode 100644 index 0000000000..3921e4cbfe --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgent.cpp @@ -0,0 +1,46 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "TurtleAgent.h" + + +// Sets default values +ATurtleAgent::ATurtleAgent() { + PrimaryActorTick.bCanEverTick = true; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.TurtleAgentController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; +} + +void ATurtleAgent::InitializeAgent() { + Super::InitializeAgent(); + RootMesh = Cast(RootComponent); +} + +// Called every frame +void ATurtleAgent::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + float ForwardForce = CommandArray[0]; + float RotForce = CommandArray[1]; + + float ThrustToApply = FMath::Clamp(ForwardForce, -MAX_THRUST, MAX_THRUST); + float YawTorqueToApply = FMath::Clamp(RotForce, -MAX_YAW, MAX_YAW); + + FVector LocalThrust = FVector(ThrustToApply, 0, 0); + LocalThrust = ConvertLinearVector(LocalThrust, ClientToUE); + + FVector LocalTorque = FVector(0, 0, YawTorqueToApply); + LocalTorque = ConvertTorque(LocalTorque, ClientToUE); + + RootMesh->AddTorqueInRadians(GetActorRotation().RotateVector(LocalTorque)); + RootMesh->AddForce(GetActorRotation().RotateVector(LocalThrust)); + + // If the turtle is upside down it is abused + if (this->GetActorUpVector().Z < -0.5) { + this->IsAbused = true; + } +} + + + diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgentController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgentController.cpp new file mode 100644 index 0000000000..b931a2695f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/TurtleAgentController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "TurtleAgentController.h" + +ATurtleAgentController::ATurtleAgentController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogTemp, Warning, TEXT("TurtleAgent Controller Initialized")); +} + +ATurtleAgentController::~ATurtleAgentController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Uav.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Uav.cpp new file mode 100644 index 0000000000..cc9fc03c92 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/Uav.cpp @@ -0,0 +1,46 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#include "Holodeck.h" +#include "Uav.h" + + +AUav::AUav() { + // Set this pawn to call Tick() every frame. You can turn this off to improve performance if you don't need it. + PrimaryActorTick.bCanEverTick = true; + + // Set the defualt controller + AIControllerClass = LoadClass(NULL, TEXT("/Script/Holodeck.UavController"), NULL, LOAD_None, NULL); + AutoPossessAI = EAutoPossessAI::PlacedInWorld; + + // TODO: the unreal unit conversion should be derived via global settings + // TODO: the physics substep doesn't seem to have updated velocity/position estimates - we likely need to access the physx body directly (e.g. https://github.com/EpicGames/UnrealEngine/pull/585) + // TODO: add changes seen in https://answers.unrealengine.com/questions/7459/question-is-120-the-engine-max-frame-rate.html + // TODO: set deltaTick to 1/40th of a second + SetActorEnableCollision(true); + // OnCalculateCustomPhysics.BindUObject(this, &AUav::SubstepTick); +} + +void AUav::InitializeAgent() { + Super::InitializeAgent(); + RootMesh = Cast(RootComponent); +} + +void AUav::Tick(float DeltaTime) { + Super::Tick(DeltaTime); + ApplyForces(); +} + +void AUav::ApplyForces() { + float RollTorqueToApply = FMath::Clamp(GetRollTorqueToApply(), -UAV_MAX_ROLL, UAV_MAX_ROLL); + float PitchTorqueToApply = FMath::Clamp(GetPitchTorqueToApply(), -UAV_MAX_PITCH, UAV_MAX_PITCH); + float YawTorqueToApply = FMath::Clamp(GetYawTorqueToApply(), -UAV_MAX_YAW_RATE, UAV_MAX_YAW_RATE); + float ThrustToApply = FMath::Clamp(GetThrustToApply(), -UAV_MAX_FORCE, UAV_MAX_FORCE); + + FVector LocalThrust = FVector(0, 0, ThrustToApply); + LocalThrust = ConvertLinearVector(LocalThrust, ClientToUE); + FVector LocalTorque = FVector(RollTorqueToApply, PitchTorqueToApply, YawTorqueToApply); + LocalTorque = ConvertTorque(LocalTorque, ClientToUE); + + // Apply torques and forces in global coordinates + RootMesh->AddTorqueInRadians(GetActorRotation().RotateVector(LocalTorque)); + RootMesh->AddForce(GetActorRotation().RotateVector(LocalThrust)); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavControlSchemeTargetRollPitch.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavControlSchemeTargetRollPitch.cpp new file mode 100644 index 0000000000..7cf5c5cfa0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavControlSchemeTargetRollPitch.cpp @@ -0,0 +1,75 @@ +#include "Holodeck.h" +#include "UavControlSchemeTargetRollPitch.h" + + +UUavControlSchemeTargetRollPitch::UUavControlSchemeTargetRollPitch(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer), + RollController(UAV_ROLL_P, UAV_ROLL_I, UAV_ROLL_D), + PitchController(UAV_PITCH_P, UAV_PITCH_I, UAV_PITCH_D), + YawController(UAV_YAW_P, UAV_YAW_I, UAV_YAW_D), + AltitudeController(UAV_ALT_P, UAV_ALT_I, UAV_ALT_D) {} + +void UUavControlSchemeTargetRollPitch::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + if (Uav == nullptr) { + Uav = static_cast(UavController->GetPawn()); + if (Uav == nullptr) { + UE_LOG(LogHolodeck, Error, TEXT("UUAVControlSchemeTargetRollPitch couldn't get Uav reference")); + return; + } + } + + float* CommandArrayFloat = static_cast(CommandArray); + float* InputCommandFloat = static_cast(InputCommand); + + float DesiredRoll = InputCommandFloat[0]; + float DesiredPitch = InputCommandFloat[1]; + float DesiredYawRate = InputCommandFloat[2]; + float DesiredAltitude = InputCommandFloat[3]; + + // Update forces and movements function + float CurrentPositionZ = Uav->GetActorLocation().Z; + + // Get the rotator to get state and transform from world to local coordinates + FRotator CurrentRotator = ConvertAngularVector(Uav->GetActorRotation(), UEToClient); + FVector EulerRotation = RotatorToEulerInZYX(CurrentRotator); // Get these in local coords in (Z, Y, X) order - CurrentRotator.Euler() provides (X, Y, Z) + FVector CurrentGlobalVelocity = ConvertLinearVector(Uav->GetVelocity(), UEToClient); + FVector LocalAngularVelocity = CurrentRotator.UnrotateVector(Uav->RootMesh->GetPhysicsAngularVelocityInDegrees()); + + float CurrentRoll = EulerRotation.X; + float CurrentPitch = EulerRotation.Y; + float CurrentGlobalVelocityZ = CurrentGlobalVelocity.Z; + + float CurrentRollRate = FMath::DegreesToRadians(LocalAngularVelocity.X); + float CurrentPitchRate = FMath::DegreesToRadians(LocalAngularVelocity.Y); + float CurrentYawRate = FMath::DegreesToRadians(LocalAngularVelocity.Z); + + // Calculate the force and torques from the PID controller + float RollTorqueToApply = RollController.ComputePIDDirect(DesiredRoll, CurrentRoll, CurrentRollRate, DeltaSeconds); + float PitchTorqueToApply = PitchController.ComputePIDDirect(DesiredPitch, CurrentPitch, CurrentPitchRate, DeltaSeconds); + float YawTorqueToApply = YawController.ComputePID(DesiredYawRate, CurrentYawRate, DeltaSeconds); + float HoverThrust = (Uav->RootMesh->GetMass() * GWorld->GetGravityZ()) / (cos(DesiredRoll) * cos(DesiredPitch)); + float ThrustToApply = AltitudeController.ComputePIDDirect(DesiredAltitude, CurrentPositionZ, CurrentGlobalVelocityZ, DeltaSeconds) + HoverThrust; + + // Calculate first-order filter + float TauRoll = (RollTorqueToApply > CommandArrayFloat[0]) ? UAV_TAU_UP_ROLL : UAV_TAU_DOWN_ROLL; + float TauPitch = (PitchTorqueToApply > CommandArrayFloat[1]) ? UAV_TAU_UP_PITCH : UAV_TAU_DOWN_PITCH; + float TauYawRate = (YawTorqueToApply > CommandArrayFloat[2]) ? UAV_TAU_UP_YAW_RATE : UAV_TAU_DOWN_YAW_RATE; + float TauThrust = (ThrustToApply > CommandArrayFloat[3]) ? UAV_TAU_UP_FORCE : UAV_TAU_DOWN_FORCE; + float AlphaRoll = DeltaSeconds / (TauRoll + DeltaSeconds); + float AlphaPitch = DeltaSeconds / (TauPitch + DeltaSeconds); + float AlphaYaw = DeltaSeconds / (TauYawRate + DeltaSeconds); + float AlphaThrust = DeltaSeconds / (TauThrust + DeltaSeconds); + + CommandArrayFloat[0] = (1 - AlphaRoll) * RollTorqueToApply + AlphaRoll * RollTorqueToApply; + CommandArrayFloat[1] = (1 - AlphaPitch) * PitchTorqueToApply + AlphaPitch * PitchTorqueToApply; + CommandArrayFloat[2] = (1 - AlphaYaw) * YawTorqueToApply + AlphaYaw * YawTorqueToApply; + CommandArrayFloat[3] = (1 - AlphaThrust) * ThrustToApply + AlphaThrust * ThrustToApply; +} + +FVector UUavControlSchemeTargetRollPitch::RotatorToEulerInZYX(const FRotator& Rotator) const { + FQuat Quaternion = Rotator.Quaternion(); + float Roll = atan2(2.0 * (Quaternion.W * Quaternion.X + Quaternion.Y * Quaternion.Z), 1.0 - 2.0 * (Quaternion.X * Quaternion.X + Quaternion.Y * Quaternion.Y)); + float Pitch = asin(2.0 * (Quaternion.W * Quaternion.Y - Quaternion.Z * Quaternion.X)); + float Yaw = atan2(2.0 * (Quaternion.W * Quaternion.Z + Quaternion.X * Quaternion.Y), 1.0 - 2.0 * (Quaternion.Y * Quaternion.Z + Quaternion.Z * Quaternion.Z)); + return FVector(Roll, Pitch, Yaw); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavController.cpp new file mode 100644 index 0000000000..788cc35354 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Private/UavController.cpp @@ -0,0 +1,11 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "UavController.h" + +AUavController::AUavController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnController(ObjectInitializer) { + UE_LOG(LogTemp, Warning, TEXT("UAV Controller Initialized")); +} + +AUavController::~AUavController() {} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Android.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Android.h new file mode 100644 index 0000000000..34152aaf5e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Android.h @@ -0,0 +1,97 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "GameFramework/Pawn.h" +#include "HolodeckAgent.h" + +#include "Android.generated.h" + +UCLASS() +class HOLODECK_API AAndroid : public AHolodeckAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AAndroid(); + + static constexpr int NUM_JOINTS = 48; + static constexpr int NUM_3_AXIS_JOINTS = 18; + static constexpr int NUM_2_AXIS_JOINTS = 10; + static constexpr int NUM_1_AXIS_JOINTS = 20; + static constexpr int NUM_2_PLUS_3_AXIS_JOINTS = 28; + static constexpr int TOTAL_DOF = NUM_3_AXIS_JOINTS * 3 + NUM_2_AXIS_JOINTS * 2 + NUM_1_AXIS_JOINTS; // 94 DOF in total + + const static FName Joints[]; + const static FName BoneNames[]; + const static int NumBones; + + static constexpr float MAX_TORQUE = 30.0f; + + /** + * Called when the game starts. + */ + virtual void InitializeAgent() override; + + /** + * Tick + * Called every frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + //Decal material. This is used to show collisions on the Android. It is to be left blank and is set programmatically + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = Materials) + UMaterial* CollisionDecalMaterial; + + /** + * SetCollisionsVisible + * Sets whether collisions with the Android can be seen. + * @param Visible true to be seen. + */ + void SetCollisionsVisible(bool Visible); + + /** + * GetJoints + * Gets pointer to constant array of FName joints + * @return array of FName corresponding to android joint names + */ + const FName* GetJoints(); + + /** + * GetCollisionsVisible + * @return true if collisions are visible. + */ + bool GetCollisionsVisible(); + + UPROPERTY(BlueprintReadWrite, Category = AndroidMesh) + USkeletalMeshComponent* SkeletalMesh; + + unsigned int GetRawActionSizeInBytes() const override { + return TOTAL_DOF * sizeof(float); + } + + void* GetRawActionBuffer() const override { + return (void*)CommandArray; + } + + // Allows agent to fall up to ~10 meters + float GetAccelerationLimit() override { return 200; } + +private: + bool bCollisionsAreVisible; + + /** + * ApplyTorques + * Applies torques for that tick on each joint with a force/direction + * corresponding to the values in the command array + */ + void ApplyTorques(); + float CommandArray[TOTAL_DOF]; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidControlSchemeMaxTorque.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidControlSchemeMaxTorque.h new file mode 100644 index 0000000000..84d2d5d16e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidControlSchemeMaxTorque.h @@ -0,0 +1,52 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "Android.h" +#include "HolodeckControlScheme.h" + +#include "AndroidControlSchemeMaxTorque.generated.h" + +/** + * UAndroidControlSchemeMaxTorque + */ +UCLASS() +class HOLODECK_API UAndroidControlSchemeMaxTorque : public UHolodeckControlScheme +{ +public: + GENERATED_BODY() + + UAndroidControlSchemeMaxTorque(const FObjectInitializer& ObjectInitializer); + + void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) override; + + unsigned int GetControlSchemeSizeInBytes() const override { + return 94 * sizeof(float); + } + + void SetController(AHolodeckPawnController* const Controller) { AndroidController = Controller; }; + +private: + AHolodeckPawnController* AndroidController; + AAndroid* Android; + + const float TorqueScalarMuscles = 2.0f; // Muscles can apply more torque than fingers + const float TorqueScalarFingers = 0.9f; + const float MinCommand = -1.0f; + const float MaxCommand = 1.0f; + + /** + * calcTorqueToApply + * + * Takes the input command, the bone mass, and the specific scalar value and computes the torque to apply + * + * CommandValue is between -1.0 and 1.0. It represents the percentage of the maximum torque to apply. + * + * The maximum Torque is calculated by multiplying the bone mass and the given torque scalar. + * Bone mass is in kg, TorqueScalar is (Neutonmeters/kg) + * + */ + float calcTorqueToApply(float CommandValue, float BoneMass, float TorqueScalar=1.0f); + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidController.h new file mode 100644 index 0000000000..e4ec2569e2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/AndroidController.h @@ -0,0 +1,44 @@ +#pragma once + +#include "Holodeck.h" + +#include "Android.h" +#include "HolodeckPawnController.h" +#include "AndroidControlSchemeMaxTorque.h" +#include "PhysicsEngine/ConstraintInstance.h" +#include "JointMaxTorqueControlScheme.h" +#include "AndroidController.generated.h" + +UCLASS() +class HOLODECK_API AAndroidController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AAndroidController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~AAndroidController(); + + /** + * OnPossess + * Called when the controller possesses the pawn. + * @param Pawn the pawn being possessed. + */ + void OnPossess(APawn* Pawn) override; + + void AddControlSchemes(); + +private: + + USkeletalMeshComponent* SkeletalMeshComponent; + float* ActionBufferFloatPtr; + AHolodeckAgent* ControlledAndroid; + UJointMaxTorqueControlScheme* ControlScheme; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgent.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgent.h new file mode 100644 index 0000000000..77b1018f58 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgent.h @@ -0,0 +1,89 @@ +#pragma once + +#include + +#include "Holodeck.h" + +#include "GameFramework/Pawn.h" +#include "HolodeckAgent.h" + +#include "HandAgent.generated.h" + +UCLASS(Blueprintable) +class HOLODECK_API AHandAgent : public AHolodeckAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHandAgent(); + + static constexpr int NUM_JOINTS = 16; + static constexpr int NUM_3_AXIS_JOINTS = 1; + static constexpr int NUM_2_AXIS_JOINTS = 5; + static constexpr int NUM_1_AXIS_JOINTS = 10; + static constexpr int NUM_2_PLUS_3_AXIS_JOINTS = NUM_3_AXIS_JOINTS + NUM_2_AXIS_JOINTS; + static constexpr int NUM_FLOAT_DIRECTIONS = 3; + static constexpr int TOTAL_JOINT_DOF = NUM_3_AXIS_JOINTS * 3 + + NUM_2_AXIS_JOINTS * 2 + + NUM_1_AXIS_JOINTS; + + static constexpr int TOTAL_DOF = TOTAL_JOINT_DOF + NUM_FLOAT_DIRECTIONS; + static constexpr float MAX_MOVEMENT_METERS = 0.5; + static constexpr float MAX_TORQUE = 30; + + const static FName Joints[]; + const static FName BoneNames[]; + const static int NumBones; + + /** + * Called when the game starts. + */ + virtual void InitializeAgent() override; + + /** + * Tick + * Called every frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + + /** + * GetJoints + * Gets pointer to constant array of FName joints + * @return array of FName corresponding to android joint names + */ + const FName* GetJoints(); + + + UPROPERTY(BlueprintReadWrite, Category = AndroidMesh) + USkeletalMeshComponent* SkeletalMesh; + + unsigned int GetRawActionSizeInBytes() const override { + return TOTAL_DOF * sizeof(float); + } + + void* GetRawActionBuffer() const override { + return (void*)CommandArray; + } + +private: + + /** + * ApplyTorques + * Applies torques for that tick on each joint with a force/direction + * corresponding to the values in the command array + */ + void ApplyTorques(); + + /** + * ApplyLevitation + * Makes the HandAgent fly around. Used with the HandAgentMaxTorqueFloat control scheme + */ + void ApplyLevitation(); + + float CommandArray[TOTAL_DOF]; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentController.h new file mode 100644 index 0000000000..9e3d98b1d5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentController.h @@ -0,0 +1,46 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" + +#include "HandAgent.h" // TODO Delete This +#include "HolodeckPawnController.h" +#include "JointMaxTorqueControlScheme.h" +#include "PhysicsEngine/ConstraintInstance.h" +#include "HandAgentMaxTorqueFloat.h" + +#include "HandAgentController.generated.h" + +UCLASS() +class HOLODECK_API AHandAgentController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHandAgentController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~AHandAgentController() {}; + + /** + * Possess + * Called when the controller possesses the pawn. + * @param Pawn the pawn being possessed. + */ + void OnPossess(APawn* Pawn) override; + + void AddControlSchemes(); + +private: + + USkeletalMeshComponent* SkeletalMeshComponent; + float* ActionBufferFloatPtr; + UJointMaxTorqueControlScheme* JointTorqueControlScheme; + UHandAgentMaxTorqueFloat* HandAgentFloatControlScheme; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentMaxTorqueFloat.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentMaxTorqueFloat.h new file mode 100644 index 0000000000..2a87e20c5d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HandAgentMaxTorqueFloat.h @@ -0,0 +1,37 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckControlScheme.h" +#include "JointMaxTorqueControlScheme.h" +#include "HandAgent.h" + +#include "HandAgentMaxTorqueFloat.generated.h" + +/** + * UHandAgentMaxTorqueFloat - JointMaxTorqueControlScheme, but the last three elements in the + * command array control the Hand agent's movement in the x, y, and z planes so it can float around. + */ +UCLASS() +class HOLODECK_API UHandAgentMaxTorqueFloat : public UHolodeckControlScheme +{ + GENERATED_BODY() + +public: + UHandAgentMaxTorqueFloat(const FObjectInitializer& ObjectInitializer) {}; + + void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) override; + + unsigned int GetControlSchemeSizeInBytes() const override; + + /** + * This control scheme delegates most of its work to this UJointMaxTorqueControlScheme, + * which must be provided with this method after initializing it. + */ + void SetTorqueControlScheme(UJointMaxTorqueControlScheme* Scheme); + +private: + UJointMaxTorqueControlScheme *TorqueControlScheme; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUV.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUV.h new file mode 100644 index 0000000000..a92bc8489c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUV.h @@ -0,0 +1,71 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "Containers/Array.h" +#include "GameFramework/Pawn.h" +#include "HolodeckBuoyantAgent.h" +#include "HoveringAUV.generated.h" + + +UCLASS() +/** +* AHoveringAUV +* Inherits from the HolodeckAgent class +* On any tick this object will apply the given forces. +* Desired values must be set by a controller. +*/ +class HOLODECK_API AHoveringAUV : public AHolodeckBuoyantAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + AHoveringAUV(); + + void InitializeAgent() override; + + /** + * Tick + * Called each frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + unsigned int GetRawActionSizeInBytes() const override { return 8 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + + // Allows agent to fall up to ~8 meters + float GetAccelerationLimit() override { return 400; } + + // Location of all thrusters + TArray thrusterLocations{ FVector(18.18, 22.14, -4), + FVector(18.18, -22.14, -4), + FVector(-31.43, -22.14, -4), + FVector(-31.43, 22.14, -4), + FVector(7.39, 18.23, -0.21), + FVector(7.39, -18.23, -0.21), + FVector(-20.64, -18.23, -0.21), + FVector(-20.64, 18.23, -0.21) }; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = BuoyancySettings) + bool Perfect= true; + + void ApplyThrusters(); + +private: + /** NOTE: These go counter-clockwise, starting in front right + * 0: Vertical Front Starboard Thruster + * 1: Vertical Front Port Thruster + * 2: Vertical Back Port Thruster + * 3: Vertical Back Starboard Thruster + * 4: Angled Front Starboard Thruster + * 5: Angled Front Port Thruster + * 6: Angled Back Port Thruster + * 7: Angled Back Starboard Thruster + */ + float CommandArray[8]; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUVController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUVController.h new file mode 100644 index 0000000000..b5a712b6c9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/HoveringAUVController.h @@ -0,0 +1,34 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "HoveringAUV.h" + +#include "HoveringAUVController.generated.h" + +/** +* A Holodeck Turtle Agent Controller +*/ +UCLASS() +class HOLODECK_API AHoveringAUVController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHoveringAUVController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~AHoveringAUVController(); + + void AddControlSchemes() override { + // No control schemes + } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/JointMaxTorqueControlScheme.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/JointMaxTorqueControlScheme.h new file mode 100644 index 0000000000..e4d3fc2cf3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/JointMaxTorqueControlScheme.h @@ -0,0 +1,72 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "Android.h" +#include "HolodeckControlScheme.h" + +#include "JointMaxTorqueControlScheme.generated.h" + +/** + * UAndroidControlSchemeMaxTorque + */ +UCLASS() +class HOLODECK_API UJointMaxTorqueControlScheme : public UHolodeckControlScheme +{ +public: + GENERATED_BODY() + + UJointMaxTorqueControlScheme(const FObjectInitializer& ObjectInitializer); + + void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) override; + + unsigned int GetControlSchemeSizeInBytes() const override; + + /** + * Must call this method to set the control scheme size after creating this object + */ + void SetControlSchemeSizeInBytes(int size); + + /** + * Must call this method to give a reference to SkeletalMesh and an array of indexed joint names + */ + void SetSkeletalMesh(USkeletalMeshComponent* SkeletalMesh, FName Joints[]); + + /** + * Fingers are less powerful than joints, after this index we will use TorqueScalarFingers + * instead of TorqueScalarMuscles + */ + void SetFingerStartIndex(int size); + void SetJointSizes(int ThreeAxis, int TwoAxis, int OneAxis); + + void SetController(AHolodeckPawnController* const NewController) { this->Controller = NewController; }; + +private: + AHolodeckPawnController* Controller; + USkeletalMeshComponent* SkeletalMesh; + FName* JointNames; + + const float TorqueScalarMuscles = 5.0f; // Muscles can apply more torque than fingers + const float TorqueScalarFingers = 1.2f; + const float MinCommand = -1.0f; + const float MaxCommand = 1.0f; + + unsigned int ControlSchemeSizeBytes = 0; + unsigned int FingerStartIndex = 0; + unsigned int ThreeAxisJoints, TwoAxisJoints, OneAxisJoints; + + /** + * calcTorqueToApply + * + * Takes the input command, the bone mass, and the specific scalar value and computes the torque to apply + * + * CommandValue is between -1.0 and 1.0. It represents the percentage of the maximum torque to apply. + * + * The maximum Torque is calculated by multiplying the bone mass and the given torque scalar. + * Bone mass is in kg, TorqueScalar is (Neutonmeters/kg) + * + */ + float CalculateTorque(float CommandValue, float BoneMass, float TorqueScalar = 1.0f); + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgent.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgent.h new file mode 100644 index 0000000000..7271147b9a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgent.h @@ -0,0 +1,50 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckAgent.h" +#include "Holodeck.h" +#include "HolodeckAgent.h" + +#include "NavAgent.generated.h" + +UCLASS() +class HOLODECK_API ANavAgent : public AHolodeckAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + ANavAgent(); + + /** + * BeginPlay + * Called when the game starts. + * Registers the reward and terminal signals. + */ + void InitializeAgent() override; + + /** + * Tick + * Called each frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + UFUNCTION(BlueprintCallable, Category="Holodeck") + FVector GetTarget(); + + UFUNCTION(BlueprintCallable, Category = "Holodeck") + void SetTarget(float x, float y, float z); + + unsigned int GetRawActionSizeInBytes() const override { return 3 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + +private: + float CommandArray[3]; + FVector Target; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgentController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgentController.h new file mode 100644 index 0000000000..7c3de244d1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/NavAgentController.h @@ -0,0 +1,34 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "Holodeck.h" +#include "HolodeckPawnController.h" +#include "NavAgent.h" + +#include "NavAgentController.generated.h" + +/** +* A Holodeck Nav Agent Controller +*/ +UCLASS() +class HOLODECK_API ANavAgentController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + ANavAgentController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~ANavAgentController(); + + void AddControlSchemes() override { + // No Extra Control Schemes + } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/RawControlScheme.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/RawControlScheme.h new file mode 100644 index 0000000000..2097961fad --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/RawControlScheme.h @@ -0,0 +1,32 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckAgentInterface.h" +#include "HolodeckControlScheme.h" + +#include "RawControlScheme.generated.h" + +/** +* URawControlScheme +*/ +UCLASS() +class HOLODECK_API URawControlScheme : public UHolodeckControlScheme { + GENERATED_BODY() + +public: + // Constructor required by engine. Shouldn't be used + URawControlScheme(const FObjectInitializer& ObjectInitializer); + + URawControlScheme(AHolodeckAgentInterface* const ControlledAgent); + + void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) override; + + unsigned int GetControlSchemeSizeInBytes() const { + return Agent->GetRawActionSizeInBytes(); + } + + // Cannot be private and be added to TArray + UPROPERTY() + AHolodeckAgentInterface* Agent; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobot.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobot.h new file mode 100644 index 0000000000..dc650ead86 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobot.h @@ -0,0 +1,50 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckAgent.h" + +#include "SphereRobot.generated.h" + +static float MAX_ROTATION_SPEED = 20; +static float MAX_FORWARD_SPEED = 20; + +UCLASS() +class HOLODECK_API ASphereRobot : public AHolodeckAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + ASphereRobot(); + + /** + * BeginPlay + * Called when the game starts. + * Registers the reward and terminal signals. + */ + void InitializeAgent() override; + + /** + * Tick + * Called each frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float ForwardSpeed; + + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float RotSpeed; + + unsigned int GetRawActionSizeInBytes() const override { return 2 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + +private: + float CommandArray[2]; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobotController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobotController.h new file mode 100644 index 0000000000..31ad653a0a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/SphereRobotController.h @@ -0,0 +1,34 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "SphereRobot.h" + +#include "SphereRobotController.generated.h" + +/** + * A Holodeck Sphere Robot Controller + */ +UCLASS() +class HOLODECK_API ASphereRobotController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + ASphereRobotController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~ASphereRobotController(); + + void AddControlSchemes() override { + // No control schemes + } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUV.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUV.h new file mode 100644 index 0000000000..1d60f1573e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUV.h @@ -0,0 +1,71 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Containers/Array.h" +#include "Kismet/KismetMathLibrary.h" +#include "GameFramework/Pawn.h" +#include "HolodeckBuoyantAgent.h" +#include "TorpedoAUV.generated.h" + +const float TAUV_MIN_THRUST = -100; +const float TAUV_MAX_THRUST = 100; +const float TAUV_MIN_FIN = -45; +const float TAUV_MAX_FIN = 45; + +UCLASS() +/** +* ATorpedoAUV +* Inherits from the HolodeckAgent class +* On any tick this object will apply the given forces. +* Desired values must be set by a controller. +*/ +class HOLODECK_API ATorpedoAUV : public AHolodeckBuoyantAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + ATorpedoAUV(); + + void InitializeAgent() override; + + /** + * Tick + * Called each frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + unsigned int GetRawActionSizeInBytes() const override { return 5 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + + // Allows agent to fall up to ~8 meters + float GetAccelerationLimit() override { return 400; } + + // Location of all forces to apply + FVector thruster = FVector(-120,0,0); + TArray finTranslation{ FVector(-105,7.07,0), + FVector(-105,0,7.07), + FVector(-105,-7.07,0), + FVector(-105,0,-7.07) }; + TArray finRotation{ FRotator(0,0,0), + FRotator(0,0,-90), + FRotator(0,0,-180), + FRotator(0,0,-270) }; + + void ApplyFin(int i); + +private: + /** NOTE: These go counter-clockwise, starting in front right + * 0: Left Fin + * 1: Top Fin + * 2: Right Fin + * 3: Bottom Fin + * 4: Thruster + */ + float CommandArray[5]; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUVController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUVController.h new file mode 100644 index 0000000000..2c909d26c0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TorpedoAUVController.h @@ -0,0 +1,34 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "TorpedoAUV.h" + +#include "TorpedoAUVController.generated.h" + +/** +* A Holodeck Turtle Agent Controller +*/ +UCLASS() +class HOLODECK_API ATorpedoAUVController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + ATorpedoAUVController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~ATorpedoAUVController(); + + void AddControlSchemes() override { + // No control schemes + } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgent.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgent.h new file mode 100644 index 0000000000..26e96e4bc9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgent.h @@ -0,0 +1,54 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "GameFramework/Pawn.h" +#include "HolodeckAgent.h" +#include "TurtleAgent.generated.h" + +static constexpr float MAX_THRUST = 160.0f; +static constexpr float MAX_YAW = 35.0f; + +UCLASS() +/** +* ATurtleAgent +* Inherits from the HolodeckAgent class +* On any tick this object will apply the given forces. +* Desired values must be set by a controller. +*/ +class HOLODECK_API ATurtleAgent : public AHolodeckAgent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + ATurtleAgent(); + + void InitializeAgent() override; + + /** + * Tick + * Called each frame. + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + UPROPERTY(BlueprintReadWrite, Category = UAVMesh) + UStaticMeshComponent* RootMesh; + + unsigned int GetRawActionSizeInBytes() const override { return 2 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + + // Allows agent to fall up to ~8 meters + float GetAccelerationLimit() override { return 400; } + +private: + /** + * 0: ThrustToApply + * 1: YawTorqueToApply + */ + float CommandArray[2]; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgentController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgentController.h new file mode 100644 index 0000000000..39a6a10af3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/TurtleAgentController.h @@ -0,0 +1,34 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "TurtleAgent.h" + +#include "TurtleAgentController.generated.h" + +/** +* A Holodeck Turtle Agent Controller +*/ +UCLASS() +class HOLODECK_API ATurtleAgentController : public AHolodeckPawnController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + ATurtleAgentController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~ATurtleAgentController(); + + void AddControlSchemes() override { + // No control schemes + } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Uav.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Uav.h new file mode 100644 index 0000000000..b40af3c039 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/Uav.h @@ -0,0 +1,78 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#pragma once + +#include "GameFramework/Pawn.h" +#include "HolodeckAgent.h" +#include "Uav.generated.h" + +//All in radians. (rad/s, rad/s^2, etc.) +const float UAV_MASS = 3.856; //Kilograms +const float UAV_MU = 1; +const float UAV_MAX_ROLL = 6.5080; +const float UAV_MAX_PITCH = 5.087; +const float UAV_MAX_YAW_RATE = .8; +const float UAV_MAX_FORCE = 59.844; + +UCLASS() +/** +* AUav +* Inherits from the HolodeckAgent class +* On any tick this object will: +* Calculate the forces to apply using PID controllers, desired values, and current values. +* Apply the given forces. +* Desired values must be set by a controller. +*/ +class HOLODECK_API AUav : public AHolodeckAgent +{ + GENERATED_BODY() +public: + AUav(); + virtual void InitializeAgent() override; + virtual void Tick(float DeltaSeconds) override; + void SubstepTick(float DeltaTime, FBodyInstance* BodyInstance); + void UpdateForcesAndMoments(float DeltaTime); + void ApplyForces(); + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = Desired) + float DesiredAltitude; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = Desired) + float DesiredYawRate; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = Desired) + float DesiredPitch; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = Desired) + float DesiredRoll; + UPROPERTY(BlueprintReadWrite, Category = UAVMesh) + UStaticMeshComponent* RootMesh; + + //See HolodeckAgent.h for descriptions of these overriden functions + + float GetRollTorqueToApply() { return -CommandArray[0]; }; + float GetPitchTorqueToApply() { return -CommandArray[1]; }; + float GetYawTorqueToApply() { return CommandArray[2]; }; + float GetThrustToApply() { return CommandArray[3]; }; + + unsigned int GetRawActionSizeInBytes() const override { return 4 * sizeof(float); }; + void* GetRawActionBuffer() const override { return (void*)CommandArray; }; + + // Allows agent to fall up to ~9 meters + float GetAccelerationLimit() override { return 300; } + +protected: + //See HolodeckAgent.h for descriptions of these overriden functions + +private: + /** + * 0: RollTorqueToApply + * 1: PitchTorqueToApply + * 2: YawTorqueToApply + * 3: ThrustToApply + */ + float CommandArray[4]; + + FCalculateCustomPhysics OnCalculateCustomPhysics; + + /** + * InitializePIDControllers + */ + void InitializePIDControllers(); + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavControlSchemeTargetRollPitch.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavControlSchemeTargetRollPitch.h new file mode 100644 index 0000000000..f2bb01be6a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavControlSchemeTargetRollPitch.h @@ -0,0 +1,65 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "Uav.h" +#include "HolodeckControlScheme.h" +#include "SimplePID.h" +#include + +#include "UavControlSchemeTargetRollPitch.generated.h" + +const float UAV_TAU_UP_ROLL = 0.1904; +const float UAV_TAU_UP_PITCH = 0.1904; +const float UAV_TAU_UP_YAW_RATE = 0.04; // 1644; +const float UAV_TAU_UP_FORCE = 0.1644; +const float UAV_TAU_DOWN_ROLL = 0.1904; +const float UAV_TAU_DOWN_PITCH = 0.1904; +const float UAV_TAU_DOWN_YAW_RATE = 0.04;// 0.2164; +const float UAV_TAU_DOWN_FORCE = 0.2164; +const float UAV_ROLL_P = 25.0; +const float UAV_ROLL_I = 0.0; +const float UAV_ROLL_D = 8.0; +const float UAV_PITCH_P = 25.0; +const float UAV_PITCH_I = 0.0; +const float UAV_PITCH_D = 8.0; +const float UAV_YAW_P = 20.0; +const float UAV_YAW_I = 0.0; +const float UAV_YAW_D = 5.0; +const float UAV_ALT_P = 305.0; +const float UAV_ALT_I = 100.0; +const float UAV_ALT_D = 600.0; + +/** +* UUavControlSchemeTargetRollPitch +*/ +UCLASS() +class HOLODECK_API UUavControlSchemeTargetRollPitch : public UHolodeckControlScheme { +public: + GENERATED_BODY() + + UUavControlSchemeTargetRollPitch(const FObjectInitializer& ObjectInitializer); + + void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) override; + + unsigned int GetControlSchemeSizeInBytes() const override { + return 4 * sizeof(float); + } + + void SetController(AHolodeckPawnController* const Controller) { UavController = Controller; }; + +private: + FVector RotatorToEulerInZYX(const FRotator& Rotator) const; + + AHolodeckPawnController* UavController; + AUav* Uav; + + // PID Controllers + SimplePID RollController; + SimplePID PitchController; + SimplePID YawController; + SimplePID AltitudeController; + + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavController.h new file mode 100644 index 0000000000..ddf24eba52 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Agents/Public/UavController.h @@ -0,0 +1,44 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "UavControlSchemeTargetRollPitch.h" +#include "HolodeckPawnController.h" + +#include "UavController.generated.h" + +/** + * AHolodeckUAVController + * Controller for the Holodeck UAV + * Gets the commands to give the the UAV from the action buffer + * Sets the desired commands on the UAV. + */ +UCLASS() +class HOLODECK_API AUavController : public AHolodeckPawnController { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + AUavController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor. + */ + ~AUavController(); + + void AddControlSchemes() { + UUavControlSchemeTargetRollPitch* ControlScheme = NewObject(); + ControlScheme->SetController(this); + ControlSchemes.Add(ControlScheme); + } + +private: + float desiredHeight, currentHeight; + float desiredRoll, currentRoll; + float desiredPitch, currentPitch; + float desiredYawRate, currentYawRate; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AddSensorCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AddSensorCommand.cpp new file mode 100644 index 0000000000..dd5e5904b3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AddSensorCommand.cpp @@ -0,0 +1,95 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "AddSensorCommand.h" +#include "HolodeckSensor.h" + +void UAddSensorCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("UAddSensorCommand::Add sensor")); + + if (StringParams.size() != 5 || NumberParams.size() != 6) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in v. Command not executed.")); + return; + } + + AHolodeckGameMode* GameTarget = static_cast(Target); + if (GameTarget == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("UCommand::Target is not a UHolodeckGameMode*. UAddSensorCommand::Sensor not added.")); + return; + } + + UWorld* World = Target->GetWorld(); + if (World == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("UAddSensorCommand::Execute found world as nullptr. Sensor not added.")); + return; + } + + static USensorMapType SensorMap = { { "HolodeckCollisionSensor", UHolodeckCollisionSensor::StaticClass() }, + { "IMUSensor", UIMUSensor::StaticClass() }, + { "JointRotationSensor", UJointRotationSensor::StaticClass() }, + { "LocationSensor", ULocationSensor::StaticClass() }, + { "OrientationSensor", UOrientationSensor::StaticClass() }, + { "PressureSensor", UPressureSensor::StaticClass() }, + { "RelativeSkeletalPositionSensor", URelativeSkeletalPositionSensor::StaticClass() }, + { "RGBCamera", URGBCamera::StaticClass() }, + { "RotationSensor", URotationSensor::StaticClass() }, + { "VelocitySensor", UVelocitySensor::StaticClass() }, + { "AbuseSensor", UAbuseSensor::StaticClass() }, + { "ViewportCapture", UViewportCapture::StaticClass() }, + { "DistanceTask", UDistanceTask::StaticClass() }, + { "LocationTask", ULocationTask::StaticClass() }, + { "FollowTask", UFollowTask::StaticClass() }, + { "CupGameTask", UCupGameTask::StaticClass() }, + { "WorldNumSensor", UWorldNumSensor::StaticClass() }, + { "RangeFinderSensor", URangeFinderSensor::StaticClass() }, + { "CleanUpTask", UCleanUpTask::StaticClass() }, + { "DVLSensor", UDVLSensor::StaticClass() }, + { "PoseSensor", UPoseSensor::StaticClass() }, + { "AcousticBeaconSensor", UAcousticBeaconSensor::StaticClass() }, + { "ImagingSonar", UImagingSonar::StaticClass() }, + { "SidescanSonar", USidescanSonar::StaticClass() }, + { "SinglebeamSonar", USinglebeamSonar::StaticClass() }, + { "ProfilingSonar", UProfilingSonar::StaticClass() }, + { "GPSSensor", UGPSSensor::StaticClass() }, + { "DepthSensor", UDepthSensor::StaticClass() }, + { "OpticalModemSensor", UOpticalModemSensor::StaticClass() }, + }; + + FString AgentName = StringParams[0].c_str(); + FString SensorName = StringParams[1].c_str(); + FString TypeName = StringParams[2].c_str(); + FString ParmsJson = StringParams[3].c_str(); + FString SocketName = StringParams[4].c_str(); + float LocationX = NumberParams[0]; + float LocationY = NumberParams[1]; + float LocationZ = NumberParams[2]; + + FRotator Rotation = RPYToRotator(NumberParams[3], NumberParams[4], NumberParams[5]); + + AHolodeckAgent* Agent = GetAgent(AgentName); + + verifyf(Agent, TEXT("%s: Couldn't get Agent %s attaching sensor %s!"), *FString(__func__), *AgentName, *SensorName); + + UHolodeckSensor* Sensor = NewObject(Agent->GetRootComponent(), SensorMap[TypeName]); + Sensor->SensorName = SensorName; + Sensor->ParseSensorParms(ParmsJson); + Sensor->SetRelativeLocation(ConvertLinearVector(FVector(LocationX, LocationY, LocationZ), ClientToUE)); + Sensor->SetRelativeRotation(Rotation); + + if (Sensor && Agent) + { + Sensor->RegisterComponent(); + + if (SocketName.IsEmpty()) { + Sensor->AttachToComponent(Agent->GetRootComponent(), FAttachmentTransformRules::KeepRelativeTransform); + } + else { + Sensor->AttachToComponent(Agent->GetRootComponent(), FAttachmentTransformRules::KeepRelativeTransform, FName(*SocketName)); + } + + Sensor->SetAgentAndController(Agent->HolodeckController, AgentName); + Sensor->InitializeSensor(); + Agent->SensorMap.Add(Sensor->SensorName, Sensor); + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AdjustRenderQualityCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AdjustRenderQualityCommand.cpp new file mode 100644 index 0000000000..ed5fb98406 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/AdjustRenderQualityCommand.cpp @@ -0,0 +1,40 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "AdjustRenderQualityCommand.h" + +void UAdjustRenderQualityCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("UAdjustRenderQualityCommand::Execute change render quality")); + + if (StringParams.size() != 0 || NumberParams.size() != 1) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in UAdjustRenderQualityCommand. Command not executed.")); + return; + } + + UGameUserSettings* settings = GEngine->GetGameUserSettings(); + + // 0 = worst quality, 3 = best + // see + int quality = NumberParams[0]; + + if (quality < 0 || quality > 3) { + UE_LOG(LogHolodeck, Warning, TEXT("Invalid quality passed to UAdjustRenderQualityCommand!")); + return; + } + + settings->SetOverallScalabilityLevel(quality); + + if (quality == 0) { + // quality 0 will set the ScreenPercentage to 50. If the users wants to lower the resolution of the viewport or camera, + // there are better ways of doing so, so force the scale to be 1 (100%) + settings->SetResolutionScaleNormalized(1.0f); + } + + // Apply the settings (these function calls are what ApplySettings() does) without writing the config out to disk + // this is to prevent changes from persisting between holodeck instances + settings->ApplyResolutionSettings(false); + settings->ApplyNonResolutionSettings(); + + UE_LOG(LogHolodeck, Log, TEXT("UAdjustRenderQualityCommand was successful")); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/Command.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/Command.cpp new file mode 100644 index 0000000000..f39c19c385 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/Command.cpp @@ -0,0 +1,25 @@ +#include "Holodeck.h" +#include "Command.h" +#include "HolodeckGameMode.h" + +UCommand::UCommand() { + +} + +void UCommand::Init(const std::vector& NumberParameters, const std::vector& StringParameters, AActor* const ParameterTarget) { + this->NumberParams = NumberParameters; + this->StringParams = StringParameters; + this->Target = ParameterTarget; +} + +AHolodeckAgent* UCommand::GetAgent(FString AgentName) { + + AHolodeckGameMode* GameTarget = static_cast(Target); + UHolodeckServer* Server = GameTarget->GetAssociatedServer(); + if (Server->AgentMap.Contains(AgentName)) { + return Server->AgentMap[AgentName]; + } else { + UE_LOG(LogHolodeck, Error, TEXT("Unable to find the agent %s"), *AgentName); + return nullptr; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandCenter.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandCenter.cpp new file mode 100644 index 0000000000..9b26ed6d73 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandCenter.cpp @@ -0,0 +1,103 @@ +#include "Holodeck.h" +#include "CommandCenter.h" +#include "HolodeckGameMode.h" // to avoid a circular dependency. + +const FString UCommandCenter::BUFFER_NAME = "command_buffer"; +const FString UCommandCenter::BUFFER_SHOULD_READ_NAME = "command_bool"; +const int UCommandCenter::BUFFER_SHOULD_READ_SIZE = 1; +const int UCommandCenter::BUFFER_SIZE = 1048576; //one megabyte +const int UCommandCenter::BYTE_SIZE = 8; + +void UCommandCenter::GiveCommand(UCommand * const Input) { + if (Input != nullptr) + Commands.Add(Input); +} + +UCommandCenter::UCommandCenter() { + UE_LOG(LogHolodeck, Log, TEXT("CommandCenter::CommandCenter() constructed")); +} + +void UCommandCenter::Tick(float DeltaTime) { + if (ShouldReadBufferPtr && *ShouldReadBufferPtr == true) { + ReadCommandBuffer(); + *ShouldReadBufferPtr = false; + } + for (const auto &i : Commands) + i->Execute(); + Commands.Empty(); +} + +void UCommandCenter::GetCommandBuffer() { + UE_LOG(LogHolodeck, Log, TEXT("CommandCenter:: is getting command buffer")); + if (Server == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("CommandCenter could not find server...")); + } else { + Buffer = static_cast(Server->Malloc(TCHAR_TO_UTF8(*BUFFER_NAME), BUFFER_SIZE * BYTE_SIZE)); + + if (!Buffer) { + UE_LOG(LogHolodeck, Fatal, TEXT("CommandCenter::GetCommandBuffer: Failed to allocate shared memory for buffer!")); + } + + ShouldReadBufferPtr = static_cast(Server->Malloc(TCHAR_TO_UTF8(*BUFFER_SHOULD_READ_NAME), BUFFER_SHOULD_READ_SIZE * sizeof(bool))); + if (ShouldReadBufferPtr != nullptr) + *ShouldReadBufferPtr = false; + else + UE_LOG(LogHolodeck, Error, TEXT("UCommandCenter::ShouldReadBufferPtr is null")); + } +} + +void UCommandCenter::Init(UHolodeckServer* const ParameterServer, AHolodeckGameMode* const ParameterGameMode) { + this->Server = ParameterServer; + this->GameMode = ParameterGameMode; + GetCommandBuffer(); +} + +int UCommandCenter::ReadCommandBuffer() { + char *Endptr; + gason::JsonValue Value; + gason::JsonAllocator Allocator; + int Status = gason::jsonParse(Buffer, &Endptr, &Value, Allocator); + if (Status != gason::JSON_PARSE_OK) { + UE_LOG(LogHolodeck, Error, TEXT("Unable to parse command buffer as a json file")); + } else { + ExtractCommandsFromJson(Value); + } + return Status; +} + +void UCommandCenter::ExtractCommandsFromJson(const gason::JsonValue &Input){ + if (Input.getTag() == gason::JSON_OBJECT) { + gason::JsonIterator Iter = begin(Input); + //check if this is actually the array of commands, and then extract the commands from it. + if (Iter->value.getTag() == gason::JSON_ARRAY) + for (gason::JsonNode* ArrayIter : Iter->value) + GetCommand(ArrayIter->value); + else + UE_LOG(LogHolodeck, Warning, TEXT("Command Buffer didn't contain the format of JSON we expected. Unable to process command.")); + } else { + UE_LOG(LogHolodeck, Warning, TEXT("Command Buffer didn't contain the format of JSON we expected. Unable to process command.")); + } +} + +void UCommandCenter::GetCommand(const gason::JsonValue &Input) { + gason::JsonIterator Iter = begin(Input); + std::string CommandName = Iter->value.toString(); + FString CommandFString = UTF8_TO_TCHAR(CommandName.c_str()); + std::vector StringParameters; + std::vector FloatParameters; + Iter.p = Iter->next; + if (Iter->value.getTag() == gason::JSON_ARRAY) { + //We are now inside the array + for (gason::JsonNode* ArrayIter : Iter->value) { + gason::JsonIterator ObjIter = begin(ArrayIter->value); + if (ObjIter->value.getTag() == gason::JSON_NUMBER) + FloatParameters.push_back(ObjIter->value.toNumber()); + else + StringParameters.push_back(ObjIter->value.toString()); + } + } else { + UE_LOG(LogHolodeck, Warning, TEXT("Command Buffer didn't contain the format of JSON we expected. Unable to process command.")); + } + UCommand* CommandPtr = UCommandFactory::MakeCommand(CommandName, FloatParameters, StringParameters, GameMode); + this->GiveCommand(CommandPtr); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandFactory.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandFactory.cpp new file mode 100644 index 0000000000..febe32ed47 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CommandFactory.cpp @@ -0,0 +1,30 @@ +#include "Holodeck.h" +#include "CommandFactory.h" +#include "HolodeckGameMode.h" + +const static std::string SPAWN_AGENT = "SpawnAgent"; + +UCommand* UCommandFactory::MakeCommand(const std::string& Name, const std::vector& NumberParameters, const std::vector& StringParameters, AActor* ParameterGameMode) { + static UCommandMapType CommandMap = { { "SpawnAgent", &CreateInstance}, + { "TeleportCamera", &CreateInstance }, + { "RGBCameraRate", &CreateInstance }, + { "AdjustRenderQuality", &CreateInstance }, + { "DebugDraw", &CreateInstance }, + { "RenderViewport", &CreateInstance }, + { "AddSensor", &CreateInstance }, + { "RemoveSensor", &CreateInstance }, + { "RotateSensor", &CreateInstance }, + { "CustomCommand", &CreateInstance }, + { "SendAcousticMessage", &CreateInstance }, + { "SendOpticalMessage", &CreateInstance }, }; + + UCommand*(*CreateCommandFunction)() = CommandMap[Name]; + UCommand* ToReturn = nullptr; + if (CreateCommandFunction) { + ToReturn = CreateCommandFunction(); + ToReturn->Init(NumberParameters, StringParameters, ParameterGameMode); + } else { + UE_LOG(LogHolodeck, Warning, TEXT("CommandFactory failed to make command: %s"), UTF8_TO_TCHAR(Name.c_str())); + } + return ToReturn; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CustomCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CustomCommand.cpp new file mode 100644 index 0000000000..256786f6b1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/CustomCommand.cpp @@ -0,0 +1,25 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" + +void UCustomCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("Executing CustomCommand.")); + if (StringParams.size() < 1) { + UE_LOG(LogHolodeck, Error, TEXT("No command name found in CustomCommand call. Command not excecuted.")); + return; + } + FString name(StringParams[0].c_str()); + TArray nums; + for (float f : NumberParams) { + nums.Add(f); + } + TArray strs; + for (unsigned int i = 1; i < StringParams.size(); i++) { + std::string s = StringParams[i]; + strs.Add(FString(s.c_str())); + } + + AHolodeckGameMode* Game = static_cast(Target); + Game->ExecuteCustomCommand(name, nums, strs); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/DebugDrawCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/DebugDrawCommand.cpp new file mode 100644 index 0000000000..39ce609986 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/DebugDrawCommand.cpp @@ -0,0 +1,46 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "DebugDrawCommand.h" +#include "HolodeckGameMode.h" + +const float lifetime = 1.f; + +void UDebugDrawCommand::Execute() { + if (StringParams.size() != 0 || NumberParams.size() != 12) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in DebugDrawCommand. Command not executed.")); + return; + } + + AHolodeckGameMode* Game = static_cast(Target); + UWorld* World = Game->GetWorld(); + + /* + NumberParams[0] is the type of debug object to draw. 0:line, 1:arrow, 2:box, 3:point + NumberParams[1-3] are the start vector + NumberParams[4-6] are the end vector + NumberParams[7-9] are the RGB color values + NumberParams[10] is the size + NumberParams[11] is the lifetime (how long it's avalable) + */ + FVector Vec1 = FVector(NumberParams[1], NumberParams[2], NumberParams[3]); + Vec1 = ConvertLinearVector(Vec1, ClientToUE); + FVector Vec2 = FVector(NumberParams[4], NumberParams[5], NumberParams[6]); + Vec2 = ConvertLinearVector(Vec2, ClientToUE); + FColor Color = FColor(NumberParams[7], NumberParams[8], NumberParams[9]); + float lifetime = NumberParams[11]; + bool persistent = (lifetime == 0); + + // Draw debug line + if (NumberParams[0] == 0) + DrawDebugLine(World, Vec1, Vec2, Color, persistent, lifetime, 0, NumberParams[10]); + // Draw debug arrow + else if(NumberParams[0] == 1) + DrawDebugDirectionalArrow(World, Vec1, Vec2, (NumberParams[10]*10+Vec1.Dist(Vec1, Vec2)), Color, persistent, lifetime, 0, NumberParams[10]); // First float param is arrow size, second is thickness + // Draw debug box + else if (NumberParams[0] == 2) + DrawDebugBox(World, Vec1, Vec2, Color, persistent, lifetime, 0, NumberParams[10]); + // Draw debug point + else if (NumberParams[0] == 3) + DrawDebugPoint(World, Vec1, NumberParams[10], Color, persistent, lifetime, 0); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RGBCameraRateCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RGBCameraRateCommand.cpp new file mode 100644 index 0000000000..ca77c41832 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RGBCameraRateCommand.cpp @@ -0,0 +1,34 @@ +#include "Holodeck.h" +#include "RGBCameraRateCommand.h" +#include "RGBCamera.h" +#include "HolodeckGameMode.h" + +void URGBCameraRateCommand::Execute() { + + UE_LOG(LogHolodeck, Log, TEXT("RGBCameraRateCommand::Execute")); + + verifyf(StringParams.size() == 2 && NumberParams.size() == 1, TEXT("URGBCameraRateCommand::Execute: Invalid Arguments")); + + AHolodeckGameMode* GameTarget = static_cast(Target); + + verifyf(GameTarget != nullptr, TEXT("%s UCommand::Target is not a UHolodeckGameMode*."), *FString(__func__)); + + UWorld* World = Target->GetWorld(); + verify(World); + + FString AgentName = StringParams[0].c_str(); + int ticksPerCapture = NumberParams[0]; + + verifyf(ticksPerCapture > 0, TEXT("%s Invalid ticks per capture provided!"), *FString(__func__)); + + AHolodeckAgent* Agent = GetAgent(AgentName); + + verifyf(Agent, TEXT("%s Could not find agent %s"), *FString(__func__), *AgentName); + + FString SensorName = StringParams[1].c_str(); + + verifyf(Agent->SensorMap.Contains(SensorName), TEXT("%s Sensor %s not found on agent %s"), *FString(__func__), *SensorName, *AgentName); + + URGBCamera* Camera = (URGBCamera*)Agent->SensorMap[SensorName]; + Camera->TicksPerCapture = ticksPerCapture; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RemoveSensorCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RemoveSensorCommand.cpp new file mode 100644 index 0000000000..ecab376bbb --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RemoveSensorCommand.cpp @@ -0,0 +1,37 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "RemoveSensorCommand.h" + +void URemoveSensorCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("URemoveSensorCommand::Remove sensor")); + + if (StringParams.size() != 2 || NumberParams.size() != 0) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in v. Command not executed.")); + return; + } + + AHolodeckGameMode* GameTarget = static_cast(Target); + if (GameTarget == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("UCommand::Target is not a UHolodeckGameMode*. URemoveSensorCommand::Sensor not removed.")); + return; + } + + UWorld* World = Target->GetWorld(); + if (World == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("URemoveSensorCommand::Execute found world as nullptr. Sensor not removed.")); + return; + } + + FString AgentName = StringParams[0].c_str(); + FString SensorName = StringParams[1].c_str(); + AHolodeckAgent* Agent = GetAgent(AgentName); + UHolodeckSensor* Sensor = Agent->SensorMap[SensorName]; + + if (Sensor && Agent) + { + Agent->SensorMap.Remove(Sensor->SensorName); + Sensor->UnregisterComponent(); + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RenderViewportCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RenderViewportCommand.cpp new file mode 100644 index 0000000000..10059dec89 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RenderViewportCommand.cpp @@ -0,0 +1,33 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "RenderViewportCommand.h" + +void URenderViewportCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("URenderViewportCommand::Execute toggling viewport rendering")); + + if (StringParams.size() != 0 || NumberParams.size() != 1) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in URenderViewportCommand. Command not executed.")); + return; + } + + bool succeeded; + + // TODO: Apparently you aren't "supposed" to just manually send a command like this. + // It would be better to get a reference to the ViewFamily to do `ViewFamily.EngineShowFlags.Rendering = 0`; + // This is basically what the command below does. + + const FString command = TEXT("ShowFlag.Rendering "); + const FString paramater = (NumberParams[0] == 1) ? TEXT("2") : TEXT("0"); + const FString complete_command = command + paramater; + + succeeded = GEngine->Exec(GetWorld(), *complete_command); + + if (succeeded) { + UE_LOG(LogHolodeck, Log, TEXT("URenderViewportCommand was successful")); + } else { + UE_LOG(LogHolodeck, Log, TEXT("URenderViewportCommand failed!")); + } + +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RotateSensorCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RotateSensorCommand.cpp new file mode 100644 index 0000000000..140012ca74 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/RotateSensorCommand.cpp @@ -0,0 +1,33 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "RotateSensorCommand.h" + +void URotateSensorCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("URotateSensorCommand::Execute sensor")); + + if (StringParams.size() != 2 || NumberParams.size() != 3) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in URotateSensorCommand. Command not executed.")); + return; + } + + FString AgentName = StringParams[0].c_str(); + FString SensorName = StringParams[1].c_str(); + + AHolodeckAgent* Agent = GetAgent(AgentName); + verifyf(Agent, TEXT("%s: Could not find an agent with that name! %s"), *FString(__func__), *AgentName); + + verifyf(Agent->SensorMap.Contains(SensorName), TEXT("%s: Could not find a sensor with that name! %s"), *FString(__func__), *SensorName); + + UHolodeckSensor* Sensor = Agent->SensorMap[SensorName]; + + // Coordinates from the python side come in roll (x), pitch (y), yaw, (z) order + float RotationRoll = NumberParams[0]; + float RotationPitch = NumberParams[1]; + float RotationYaw = NumberParams[2]; + + UE_LOG(LogHolodeck, Log, TEXT("roll %d pitch %d yaw %d"), RotationRoll, RotationPitch, RotationYaw); + + Sensor->SetRelativeRotation(FRotator(RotationPitch, RotationYaw, RotationRoll)); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendAcousticMessageCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendAcousticMessageCommand.cpp new file mode 100644 index 0000000000..4e62c8868e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendAcousticMessageCommand.cpp @@ -0,0 +1,38 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#include "Holodeck.h" +#include "SendAcousticMessageCommand.h" +#include "AcousticBeaconSensor.h" +#include "HolodeckGameMode.h" + +void USendAcousticMessageCommand::Execute() { + + UE_LOG(LogHolodeck, Log, TEXT("SendAcousticMessageCommand::Execute")); + + // Verify things and get everything set up + verifyf(StringParams.size() == 4 && NumberParams.size() == 0, TEXT("USendAcousticMessageCommand::Execute: Invalid Arguments")); + AHolodeckGameMode* GameTarget = static_cast(Target); + verifyf(GameTarget != nullptr, TEXT("%s UCommand::Target is not a UHolodeckGameMode*."), *FString(__func__)); + UWorld* World = Target->GetWorld(); + verify(World); + + // Get sensor it came from + FString fromAgentName = StringParams[0].c_str(); + FString fromSensorName = StringParams[1].c_str(); + + AHolodeckAgent* fromAgent = GetAgent(fromAgentName); + verifyf(fromAgent, TEXT("%s Could not find agent %s"), *FString(__func__), *fromAgentName); + verifyf(fromAgent->SensorMap.Contains(fromSensorName), TEXT("%s Sensor %s not found on agent %s"), *FString(__func__), *fromSensorName, *fromAgentName); + UAcousticBeaconSensor* fromSensor = (UAcousticBeaconSensor*)fromAgent->SensorMap[fromSensorName]; + + // Get sensor where it's going + FString toAgentName = StringParams[2].c_str(); + FString toSensorName = StringParams[3].c_str(); + + AHolodeckAgent* toAgent = GetAgent(toAgentName); + verifyf(toAgent, TEXT("%s Could not find agent %s"), *FString(__func__), *toAgentName); + verifyf(toAgent->SensorMap.Contains(toSensorName), TEXT("%s Sensor %s not found on agent %s"), *FString(__func__), *toSensorName, *toAgentName); + UAcousticBeaconSensor* toSensor = (UAcousticBeaconSensor*)toAgent->SensorMap[toSensorName]; + + // Send the message + toSensor->fromSensor = fromSensor; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendOpticalMessageCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendOpticalMessageCommand.cpp new file mode 100644 index 0000000000..4273b27476 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SendOpticalMessageCommand.cpp @@ -0,0 +1,38 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#include "Holodeck.h" +#include "SendOpticalMessageCommand.h" +#include "OpticalModemSensor.h" +#include "HolodeckGameMode.h" + +void USendOpticalMessageCommand::Execute() { + + UE_LOG(LogHolodeck, Log, TEXT("SendOpticalMessageCommand::Execute")); + + // Verify things and get everything set up + verifyf(StringParams.size() == 4 && NumberParams.size() == 0, TEXT("USendOpticalMessageCommand::Execute: Invalid Arguments")); + AHolodeckGameMode* GameTarget = static_cast(Target); + verifyf(GameTarget != nullptr, TEXT("%s UCommand::Target is not a UHolodeckGameMode*."), *FString(__func__)); + UWorld* World = Target->GetWorld(); + verify(World); + + // Get sensor it came from + FString FromAgentName = StringParams[0].c_str(); + FString FromSensorName = StringParams[1].c_str(); + + AHolodeckAgent* FromAgent = GetAgent(FromAgentName); + verifyf(FromAgent, TEXT("%s Could not find agent %s"), *FString(__func__), *FromAgentName); + verifyf(FromAgent->SensorMap.Contains(FromSensorName), TEXT("%s Sensor %s not found on agent %s"), *FString(__func__), *FromSensorName, *FromAgentName); + UOpticalModemSensor* FromSensor = (UOpticalModemSensor*)FromAgent->SensorMap[FromSensorName]; + + // Get sensor where it's going + FString ToAgentName = StringParams[2].c_str(); + FString ToSensorName = StringParams[3].c_str(); + + AHolodeckAgent* ToAgent = GetAgent(ToAgentName); + verifyf(ToAgent, TEXT("%s Could not find agent %s"), *FString(__func__), *ToAgentName); + verifyf(ToAgent->SensorMap.Contains(ToSensorName), TEXT("%s Sensor %s not found on agent %s"), *FString(__func__), *ToSensorName, *ToAgentName); + UOpticalModemSensor* ToSensor = (UOpticalModemSensor*)ToAgent->SensorMap[ToSensorName]; + + // Send the message + ToSensor->FromSensor = FromSensor; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SpawnAgentCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SpawnAgentCommand.cpp new file mode 100644 index 0000000000..a7271eb583 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/SpawnAgentCommand.cpp @@ -0,0 +1,46 @@ +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "SpawnAgentCommand.h" + +void USpawnAgentCommand::Execute() { + + UE_LOG(LogHolodeck, Log, TEXT("SpawnAgentCommand::Execute spawning agent")); + verifyf(StringParams.size() == 2 && NumberParams.size() == 7, TEXT("%s: Bad Arguments"), *FString(__func__)); + + AHolodeckGameMode* GameTarget = static_cast(Target); + if (GameTarget == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("UCommand::Target is not a UHolodeckGameMode*. SpawnAgentCommand::Execute Cannot spawn agent without this pointer!")); + return; + } + + UWorld* World = Target->GetWorld(); + if (World == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("SpawnAgentCommand::Execute found world as nullptr. Unable to spawn agent!")); + return; + } + + FString AgentType = StringParams[0].c_str(); + FString AgentName = StringParams[1].c_str(); + FVector Location = FVector(NumberParams[0], NumberParams[1], NumberParams[2]); + + FRotator Rotation = RPYToRotator(NumberParams[3], NumberParams[4], NumberParams[5]); + bool IsMainAgent = (bool) NumberParams[6]; + + Location = ConvertLinearVector(Location, ClientToUE); + + // SpawnAgent command is defined in the HolodeckGameMode blueprint class and can only be edited/seen in the blueprint + AHolodeckAgent* SpawnedAgent = GameTarget->SpawnAgent(AgentType, Location, Rotation, AgentName, IsMainAgent); + verifyf(SpawnedAgent, TEXT("%s SpawnAgentCommand did not spawn a new Agent."), *FString(__func__)); + + AHolodeckPawnController* SpawnedController = nullptr; + + SpawnedAgent->AgentName = AgentName; + SpawnedAgent->MainAgent = IsMainAgent; + SpawnedAgent->SpawnDefaultController(); + SpawnedController = static_cast(SpawnedAgent->Controller); + SpawnedController->SetServer(GameTarget->GetAssociatedServer()); + SpawnedAgent->InitializeAgent(); + + UE_LOG(LogHolodeck, Log, TEXT("SpawnAgentCommand spawned a new Agent.")); + +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/TeleportCameraCommand.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/TeleportCameraCommand.cpp new file mode 100644 index 0000000000..24968b0b4f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Private/TeleportCameraCommand.cpp @@ -0,0 +1,25 @@ +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "TeleportCameraCommand.h" + +void UTeleportCameraCommand::Execute() { + UE_LOG(LogHolodeck, Log, TEXT("TeleportCameraCommand::Execute teleport camera command")); + + if (StringParams.size() != 0) { + UE_LOG(LogHolodeck, Error, TEXT("Unexpected argument length found in TeleportCameraCommand. Command not executed.")); + return; + } + + UWorld* World = Target->GetWorld(); + float UnitsPerMeter = World->GetWorldSettings()->WorldToMeters; + FVector Location = FVector(NumberParams[0], NumberParams[1], NumberParams[2]); + Location = ConvertLinearVector(Location, ClientToUE); + // Not actually a roll, pitch, yaw rotation, but where the axis the viewport should be looking along + FVector Rotation = FVector(NumberParams[3], NumberParams[4], NumberParams[5]); + Rotation = ConvertAngularVector(Rotation, ClientToUE); + + + AHolodeckGameMode* Game = static_cast(Target); + + Game->TeleportCamera(Location, Rotation); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AddSensorCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AddSensorCommand.h new file mode 100644 index 0000000000..cadb9ad875 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AddSensorCommand.h @@ -0,0 +1,59 @@ +#pragma once + +#include "Holodeck.h" + +#include +#include "HolodeckCollisionSensor.h" +#include "IMUSensor.h" +#include "JointRotationSensor.h" +#include "LocationSensor.h" +#include "OrientationSensor.h" +#include "PressureSensor.h" +#include "RelativeSkeletalPositionSensor.h" +#include "RGBCamera.h" +#include "RotationSensor.h" +#include "VelocitySensor.h" +#include "WorldNumSensor.h" +#include "AbuseSensor.h" +#include "RangeFinderSensor.h" +#include "ViewportCapture.h" +#include "DistanceTask.h" +#include "LocationTask.h" +#include "FollowTask.h" +#include "CupGameTask.h" +#include "CleanUpTask.h" +#include "DVLSensor.h" +#include "PoseSensor.h" +#include "AcousticBeaconSensor.h" +#include "ImagingSonar.h" +#include "SidescanSonar.h" +#include "ProfilingSonar.h" +#include "SinglebeamSonar.h" +#include "GPSSensor.h" +#include "DepthSensor.h" +#include "OpticalModemSensor.h" + +#include "Command.h" +#include "AddSensorCommand.generated.h" + +/** +* AddSensorCommand +* Command used to add a sensor to an agent +* Warning: This command is meant for initialization. Adding a sensor with the same name as a previously +* removed sensor may cause problems. +* +* StringParameters expects five arguments: the agent name, sensor name, sensor class, sensor parameters, and socket. +* NumberParameters expects six arguments: locations x, y, and z and rotations pitch, yaw, and roll. +*/ +UCLASS() +class HOLODECK_API UAddSensorCommand : public UCommand +{ + GENERATED_BODY() + + typedef std::map USensorMapType; + +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AdjustRenderQualityCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AdjustRenderQualityCommand.h new file mode 100644 index 0000000000..f1796c42cf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/AdjustRenderQualityCommand.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "AdjustRenderQualityCommand.generated.h" + +/** +* AdjustRenderQualityCommand +* The command used to alter the fidelity of the simulation. +* +* StringParameters are expected to be empty. +* NumberParameters are expected to be an integer between 0 and 3 inclusive +* +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API UAdjustRenderQualityCommand : public UCommand +{ + GENERATED_BODY() + +public: + // See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/Command.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/Command.h new file mode 100644 index 0000000000..6eb4c81815 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/Command.h @@ -0,0 +1,59 @@ +#pragma once + +#include "Holodeck.h" + +#include +#include "HolodeckAgent.h" +#include "GameFramework/Actor.h" +#include "Command.generated.h" + +/** + * UCommand + * This is the abstract base class for commands in the holodeck + * When instantiating an inherited command, you must call the Init function. + * - Note: commands are more safely instantiated from the commandfactory. + * When inheriting from this class, you must: + * - Implement the execute function. + * - Add the inhereted class and its name string to the CommandFactory.h file. + */ +UCLASS(ClassGroup = (Custom), abstract) +class HOLODECK_API UCommand : public UObject { + GENERATED_BODY() + +public: + + /** + * Execute + * The command performs its function when this is called. + * Override this function with what you want the child command to do. + * Use NumberParams, StringParams, and Target as input variables for the execute function. + */ + virtual void Execute() { check( 0 && "You must override UCommand::Execute" ); }; + + /** + * UCommand + * Default constructor. + */ + UCommand(); + + /** + * Init + * This must be called after creating a new command object in order to load it with the correct data. + * @param NumberParamters The numbers (represented as floats) to be used as parameters for the execute call. + * @param StringParameters the strings to be used as parameters for the execute call. + * @param TargetParameter The target of the execute function call. Usually will be the GameMode pointer. + */ + virtual void Init(const std::vector& NumberParameters, const std::vector& StringParameters, AActor* const TargetParameter); + + /** + * GetAgent + * This returns a pointer to a HolodeckAgent from the HolodeckServer given an AgentName + * @param AgentName The FString name of the agent to grab + */ + AHolodeckAgent* GetAgent(FString AgentName); + +protected: + std::vector NumberParams; + std::vector StringParams; + AActor* Target; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandCenter.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandCenter.h new file mode 100644 index 0000000000..b1b35c3e1f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandCenter.h @@ -0,0 +1,105 @@ +#pragma once + +#include "Holodeck.h" + +#include +#include + +#include "Command.h" +#include "CommandFactory.h" +#include "gason.h" +#include "HolodeckServer.h" +#include "CommandCenter.generated.h" + +class AHolodeckGameMode; //forward declare to avoid circular dependency. + + /** + * UCommandCenter + * It subscribes a memory space to receive commands from the client binding. + * It expects the memory to be formatted in UTF-8 JSON. + * When creating a new commandcenter object, you must call the init after or it will not function. + * It requires a valid pointer to the server to be initialized, and the currrent gamemode object to continue running correctly. + */ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API UCommandCenter : public UObject { + GENERATED_BODY() + +public: + + /** + * UCommandCenter + * Default constructor + */ + UCommandCenter(); + + /** + * GiveCommand + * It is used to execute whatever queued up commands there are. + * @param InputCommand A pointer to a command. The given pointer will be null after this function call. + */ + virtual void GiveCommand(UCommand* const InputCommand); + + /** + *Tick + * It is used to execute whatever queued up commands there are. + * It also checks for commands that are in the json buffer. + * If you write to the buffer, make sure to set the shouldreadbufferptr to true. + * This should be called by UHolodeckGameMode. + * @param DeltaTime How much time has passed since the last tick. + */ + virtual void Tick(float DeltaTime); + + /** + *Init + * Always make sure to call this function after instantiating a CommandCenter + * It opens a channel for receiving commands in json format. It parses them and then issues the commands. + * @param Server The holodeck server for the game. + * @param GameMode The game mode for the instance. This pointer is needed for giving commands. + */ + void Init(UHolodeckServer* const Server, AHolodeckGameMode* const GameMode); + +private: + + /** + *GetCommandBuffer + * Sets up the buffer used for pasing json commands to the command center. + * also sets up the bool buffer. + */ + virtual void GetCommandBuffer(); + + /** + *ReadCommandBuffer + * Reads the buffer, parses the resulting json, and gives the commands to the command queue. + * @return the status of the read/parse + */ + int ReadCommandBuffer(); + + UPROPERTY() + TArray Commands; + char* Buffer; + bool* ShouldReadBufferPtr; + UHolodeckServer* Server; + AHolodeckGameMode* GameMode; + const static FString BUFFER_NAME; + const static FString BUFFER_SHOULD_READ_NAME; + const static int BUFFER_SHOULD_READ_SIZE; + const static int BUFFER_SIZE; + const static int BYTE_SIZE; + + /** + * ExctractCommandsFromJson + * Reads the buffer, parses the resulting json, and gives the commands to the command queue. + * It will not succeed if the json object is not in the ofrmat expected. + * At a lower level, it calls GetCommand on every json object that in an array expected to be a command. + * @param Input the Json to extract commands from. + */ + void ExtractCommandsFromJson(const gason::JsonValue &Input); + + /** + * GetCommand + * Traverses a specific json object that is a command. + * Separates the parameters into string parameters and number parameters, then pushes the command to the Commands array. + */ + void GetCommand(const gason::JsonValue &Input); +}; + diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandFactory.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandFactory.h new file mode 100644 index 0000000000..24d0c48614 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CommandFactory.h @@ -0,0 +1,57 @@ +#pragma once + +#include "Holodeck.h" + +#include +#include + +#include "Command.h" +#include "SpawnAgentCommand.h" +#include "TeleportCameraCommand.h" +#include "RGBCameraRateCommand.h" +#include "DebugDrawCommand.h" +#include "RenderViewportCommand.h" +#include "AdjustRenderQualityCommand.h" +#include "CustomCommand.h" +#include "AddSensorCommand.h" +#include "RemoveSensorCommand.h" +#include "RotateSensorCommand.h" +#include "SendAcousticMessageCommand.h" +#include "SendOpticalMessageCommand.h" + +#include "CommandFactory.generated.h" + +class AHolodeckGameMode; + +/** + * UCommandFactory + * This is the class that should be used to instantiate UCommand objects. Feed it the name of the command along with + * the parameters that the command will need to execute. If the parameters are not needed, then give it nullptr or + * empty vectors and it will work fine. + * The purpose of this was to separate knowledge of specific commands from the command center, to remove circular + * dependencies, and to give an easy way of spawning commands. + * When you make a new command, make sure to add it to the CommandMap in the MakeCommand function in the cpp file + */ +UCLASS(ClassGroup = (Custom), abstract) +class HOLODECK_API UCommandFactory : public UObject { + GENERATED_BODY() + + typedef std::map UCommandMapType; + +public: + /** + * MakeCommand + * This is the factory method for producing commands. + */ + static UCommand* MakeCommand(const std::string& Name, const std::vector& NumberParameters, const std::vector& StringParameters, AActor* ParameterGameMode); + +private: + /** + * UCommandFactory + * Default constructor. Should not be instantiated, hence it is private. + */ + UCommandFactory() {}; + + template + static UCommand* CreateInstance() { return NewObject(); } +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CustomCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CustomCommand.h new file mode 100644 index 0000000000..b53d6920ce --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/CustomCommand.h @@ -0,0 +1,25 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "ClientCommands/Public/Command.h" +#include "CustomCommand.generated.h" + +/** +* CustomCommand +* This command is inherited by a blueprint. It is to be used to easily implement simple commands for +* any number of different functions. +* +* StringParameters expect at least 1 argument as the name of the command and any number of additional string params. +* NumberParameters of any size. +*/ +UCLASS() +class HOLODECK_API UCustomCommand : public UCommand +{ + GENERATED_BODY() + +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/DebugDrawCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/DebugDrawCommand.h new file mode 100644 index 0000000000..9b9d8e99cb --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/DebugDrawCommand.h @@ -0,0 +1,28 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "DrawDebugHelpers.h" +#include "DebugDrawCommand.generated.h" + +/** +* DebugDrawCommand +* Command used to draw debug objects in the world. +* +* StringParameters are expected to be empty. +* NumberParameters is expected to be length 12 and of the format [func_type, vector1[3], vector2[3], color[3], thickness/size, lifetime] +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API UDebugDrawCommand : public UCommand +{ + GENERATED_BODY() + + +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; + +private: + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RGBCameraRateCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RGBCameraRateCommand.h new file mode 100644 index 0000000000..869f3dd7f9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RGBCameraRateCommand.h @@ -0,0 +1,18 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "RGBCameraRateCommand.generated.h" + +/** +* RGBCameraRateCommand +* +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API URGBCameraRateCommand : public UCommand { + GENERATED_BODY() +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RemoveSensorCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RemoveSensorCommand.h new file mode 100644 index 0000000000..d2dd8962bf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RemoveSensorCommand.h @@ -0,0 +1,24 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "RemoveSensorCommand.generated.h" + +/** +* RemoveSensorCommand +* Command used to remove a sensor from an agent +* Use of this command should be infrequent. +* +* StringParameters expect two arguments, the agent name, and sensor name. +*/ +UCLASS() +class HOLODECK_API URemoveSensorCommand : public UCommand +{ + GENERATED_BODY() + +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RenderViewportCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RenderViewportCommand.h new file mode 100644 index 0000000000..1f212c6c1d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RenderViewportCommand.h @@ -0,0 +1,23 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "RenderViewportCommand.generated.h" + +/** +* RenderViewportCommand +* Command used to toggle rendering the viewport +* +* StringParameters are expected to be empty. +* NumberParameters are expected to be 1 to enable rendering and 0 to disable rendering. +* +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API URenderViewportCommand : public UCommand +{ + GENERATED_BODY() + +public: + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RotateSensorCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RotateSensorCommand.h new file mode 100644 index 0000000000..2bd5446219 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/RotateSensorCommand.h @@ -0,0 +1,26 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" +#include "Command.h" +#include "RotateSensorCommand.generated.h" + +/** +* RotateSensorCommand +* Command used to rotate a sensor on an agent +* +* StringParameters expect two arguments, the agent and sensor names. +* NumberParameters expect three arguments, representing the rotation +*/ +UCLASS() +class HOLODECK_API URotateSensorCommand : public UCommand +{ + GENERATED_BODY() + + public: + //See UCommand for the documentation of this overridden function. + void Execute() override; + + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendAcousticMessageCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendAcousticMessageCommand.h new file mode 100644 index 0000000000..e3ffdfdc0b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendAcousticMessageCommand.h @@ -0,0 +1,19 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "SendAcousticMessageCommand.generated.h" + +/** +* SendAcousticMessageCommand +* +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API USendAcousticMessageCommand : public UCommand { + GENERATED_BODY() +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendOpticalMessageCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendOpticalMessageCommand.h new file mode 100644 index 0000000000..fe3b1a35d7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SendOpticalMessageCommand.h @@ -0,0 +1,19 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "SendOpticalMessageCommand.generated.h" + +/** +* SendOpticalMessageCommand +* +*/ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API USendOpticalMessageCommand : public UCommand { + GENERATED_BODY() +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SpawnAgentCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SpawnAgentCommand.h new file mode 100644 index 0000000000..f0ca389b20 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/SpawnAgentCommand.h @@ -0,0 +1,26 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "SpawnAgentCommand.generated.h" + +/** + * SpawnAgentCommand + * The command used to spawn agents in the world. + * + * StringParameters are expected to be AgentType, and then Name. + * NumberParameters are expected to be X, Y, then Z coords of the location to spawn at. + * + * To enable it to spawn other agents you will need to: + * 1. Go to the HolodeckGameModeBlueprint in editor + * 2. Go to the SpawnAgent Function and select the AgentBpMap data field + * 3. Add the agent's name and bp reference to the map + */ +UCLASS(ClassGroup = (Custom)) +class HOLODECK_API USpawnAgentCommand : public UCommand { + GENERATED_BODY() +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/TeleportCameraCommand.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/TeleportCameraCommand.h new file mode 100644 index 0000000000..2dc0d4c5ec --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/ClientCommands/Public/TeleportCameraCommand.h @@ -0,0 +1,27 @@ +#pragma once + +#include "Holodeck.h" + +#include "Command.h" +#include "TeleportCameraCommand.generated.h" + +/** +* TeleportCameraCommand +* The command used to change the location and rotation of the camera +* +* StringParameters are expected to be empty. +* NumberParameters are expected to have 6 entries, the first three representing the location and the last three representing +* the rotation +* +*/ +UCLASS() +class HOLODECK_API UTeleportCameraCommand : public UCommand +{ + GENERATED_BODY() + +public: + //See UCommand for the documentation of this overridden function. + void Execute() override; + + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Conversion.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Conversion.cpp new file mode 100644 index 0000000000..b87914257d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Conversion.cpp @@ -0,0 +1,170 @@ +#include "Holodeck.h" +#include "Conversion.h" + +const bool USE_RHS = true; + +FRotator RPYToRotator(float Roll, float Pitch, float Yaw){ + /* Takes right-handed Roll, Pitch, Yaw, and creates left-handed rotation + about X, Y, Z in the fixed frame, or Z, Y, X about current frame. + + I don't trust UE4 with rotations after I found this: + https://github.com/EpicGames/UnrealEngine/blob/release/Engine/Source/Runtime/Core/Public/Math/Rotator.h#L484 + (Combining 2 rotations by adding euler angle's *shiver*) + */ + FMatrix R; + + float SP, SY, SR; + float CP, CY, CR; + FMath::SinCos(&SP, &CP, FMath::DegreesToRadians(Pitch)); + FMath::SinCos(&SY, &CY, FMath::DegreesToRadians(Yaw)); + FMath::SinCos(&SR, &CR, FMath::DegreesToRadians(Roll)); + + // Column, then row index + // First (X) Column + R.M[0][0] = CY*CP; + R.M[0][1] = CP*SY; + R.M[0][2] = -SP; + + // Second (Y) Column + R.M[1][0] = CY*SP*SR - CR*SY; + R.M[1][1] = CY*CR + SY*SP*SR; + R.M[2][1] = CP*SR; + + // Third (Z) Column + R.M[2][0] = SY*SR + CY*CR*SP; + R.M[2][1] = CR*SY*SP - CY*SR; + R.M[2][2] = CP*CR; + + // Flip y-axes in and out to make it left-handed + R.M[0][1] *= -1; + R.M[1][0] *= -1; + R.M[2][1] *= -1; + R.M[1][2] *= -1; + + return R.Rotator(); +} + +FVector RotatorToRPY(FRotator Rot){ + /* Takes left-handed rotation about X, Y, Z in the fixed frame, + or Z, Y, X about current frame and creates a right-handed Roll, Pitch, Yaw. + + Taken from here: http://eecs.qmul.ac.uk/~gslabaugh/publications/euler.pdf + */ + FMatrix R = FRotationMatrix::Make(Rot); + + // Flip y-axes in and out to make it right-handed + R.M[0][1] *= -1; + R.M[1][0] *= -1; + R.M[2][1] *= -1; + R.M[1][2] *= -1; + + float R31 = R.M[0][2]; + float R12 = R.M[1][0]; + float R13 = R.M[2][0]; + float theta, phi, psi; + // If we're in a singularity, assume phi = 0 + if(FMath::IsNearlyEqual(R31, -1.0f)){ + phi = 0; + theta = 90; + psi = phi + UKismetMathLibrary::DegAtan2(R12, R13); + } + else if(FMath::IsNearlyEqual(R31, 1.0f)){ + phi = 0; + theta = -90; + psi = phi + UKismetMathLibrary::DegAtan2(-R12, -R13); + } + // Otherwise do normal calculations + else{ + float R11 = R.M[0][0]; + float R21 = R.M[0][1]; + float R32 = R.M[1][2]; + float R33 = R.M[2][2]; + + theta = -UKismetMathLibrary::DegAsin(R31); + float ct = UKismetMathLibrary::DegCos(theta); + + psi = UKismetMathLibrary::DegAtan2(R32/ct, R33/ct); + phi = UKismetMathLibrary::DegAtan2(R21/ct, R11/ct); + } + // roll, pitch, yaw + FVector temp(psi, theta, phi); + UE_LOG(LogHolodeck, Warning, TEXT("Euler: %s"), *temp.ToString()); + return FVector(psi, theta, phi); +} + +FVector ConvertLinearVector(FVector Vector, ConvertType Type) { + + float ScaleFactor = 1.0; + if (Type == UEToClient) + ScaleFactor /= UEUnitsPerMeter; + else if(Type == ClientToUE) + ScaleFactor *= UEUnitsPerMeter; + + Vector.X *= ScaleFactor; + Vector.Y *= ScaleFactor; + Vector.Z *= ScaleFactor; + + if(USE_RHS) + Vector.Y *= -1; + + return Vector; +} + +FVector ConvertAngularVector(FVector Vector, ConvertType Type) { + + float ScaleFactor = 1.0; + if (Type == UEToClient) + ScaleFactor /= UEUnitsPerMeter; + else if (Type == ClientToUE) + ScaleFactor *= UEUnitsPerMeter; + + Vector.X *= ScaleFactor; + Vector.Y *= ScaleFactor; + Vector.Z *= ScaleFactor; + + if (USE_RHS) { + Vector.X *= -1; + Vector.Z *= -1; + } + + return Vector; +} + + +FRotator ConvertAngularVector(FRotator Rotator, ConvertType Type) { + + if (USE_RHS) { + Rotator.Roll *= -1; + Rotator.Yaw *= -1; + } + + return Rotator; +} + +FVector ConvertTorque(FVector Vector, ConvertType Type) { + + float ScaleFactor = 1.0; + if (Type == UEToClient) + ScaleFactor /= UEUnitsPerMeterSquared; + else if (Type == ClientToUE) + ScaleFactor *= UEUnitsPerMeterSquared; + + Vector.X *= ScaleFactor; + Vector.Y *= ScaleFactor; + Vector.Z *= ScaleFactor; + + if (USE_RHS) { + Vector.X *= -1; + Vector.Z *= -1; + } + + return Vector; +} + +float ConvertClientDistanceToUnreal(float client) { + return client * UEUnitsPerMeter; +} + +float ConvertUnrealDistanceToClient(float unreal) { + return unreal / UEUnitsPerMeter; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/HolodeckViewportClient.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/HolodeckViewportClient.cpp new file mode 100644 index 0000000000..cc4c27123d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/HolodeckViewportClient.cpp @@ -0,0 +1,24 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckViewportClient.h" +#include "HolodeckCamera.h" //Included here to avoid cyclic dependency. + +UHolodeckViewportClient::UHolodeckViewportClient(const class FObjectInitializer& PCIP) : Super(PCIP) {} + +void UHolodeckViewportClient::HolodeckTakeScreenShot() { + if (Buffer != nullptr) { + GetViewportSize(ViewportSize); + if (ViewportSize.X <= 0 || ViewportSize.Y <= 0) return; + bool bGotScreenshot = Viewport->ReadPixelsPtr(Buffer, FReadSurfaceDataFlags(RCM_UNorm, CubeFace_MAX), FIntRect(0, 0, ViewportSize.X, ViewportSize.Y)); + } +} + +void UHolodeckViewportClient::Draw(FViewport * ViewportParam, FCanvas * SceneCanvas) { + Super::Draw(ViewportParam, SceneCanvas); + HolodeckTakeScreenShot(); +} + +void UHolodeckViewportClient::SetBuffer(void* NewBuffer) { + this->Buffer = static_cast(NewBuffer); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp new file mode 100644 index 0000000000..8f8d2ca51f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/Octree.cpp @@ -0,0 +1,392 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + + +#include "Octree.h" + +// Initialize static variables +// Used when making octree +TArray Octree::corners = {FVector( 1, 1, 1), + FVector( 1, 1, -1), + FVector( 1, -1, 1), + FVector( 1, -1, -1), + FVector(-1, 1, 1), + FVector(-1, 1, -1), + FVector(-1, -1, 1), + FVector(-1, -1, -1)}; +TArray Octree::sides = {FVector( 0, 0, 1), + FVector( 0, 0,-1), + FVector( 0, 1, 0), + FVector( 0,-1, 0), + FVector( 1, 0, 0), + FVector(-1, 0, 0)}; +float Octree::cornerSize = 0.01; +FCollisionQueryParams Octree::params = Octree::init_params(); + +// Misc constants +float Octree::OctreeRoot; +float Octree::OctreeMax; +float Octree::OctreeMin; +FVector Octree::EnvMin; +FVector Octree::EnvMax; +FVector Octree::EnvCenter; +UWorld* Octree::World; +TDiscardableKeyValueCache Octree::materials; + +float sign(float val){ + bool s = signbit(val); + if(s) return -1.0; + else return 1.0; +} + +void Octree::initOctree(UWorld* w){ + World = w; + + // Load environment size + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinX="), EnvMin.X)) EnvMin.X = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinY="), EnvMin.Y)) EnvMin.Y = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMinZ="), EnvMin.Z)) EnvMin.Z = -10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxX="), EnvMax.X)) EnvMax.X = 10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxY="), EnvMax.Y)) EnvMax.Y = 10; + if (!FParse::Value(FCommandLine::Get(), TEXT("EnvMaxZ="), EnvMax.Z)) EnvMax.Z = 10; + // Clean environment size + EnvMin = ConvertLinearVector(EnvMin, ClientToUE); + EnvMax = ConvertLinearVector(EnvMax, ClientToUE); + FVector min = FVector((int)FGenericPlatformMath::Min(EnvMin.X, EnvMax.X), (int)FGenericPlatformMath::Min(EnvMin.Y, EnvMax.Y), (int)FGenericPlatformMath::Min(EnvMin.Z, EnvMax.Z)); + FVector max = FVector((int)FGenericPlatformMath::Max(EnvMin.X, EnvMax.X), (int)FGenericPlatformMath::Max(EnvMin.Y, EnvMax.Y), (int)FGenericPlatformMath::Max(EnvMin.Z, EnvMax.Z)); + EnvMin = min; + EnvMax = max; + UE_LOG(LogHolodeck, Log, TEXT("Octree:: EnvMin: %s"), *EnvMin.ToString()); + UE_LOG(LogHolodeck, Log, TEXT("Octree:: EnvMax: %s"), *EnvMax.ToString()); + + // Get octree min/max + float tempVal; + if (!FParse::Value(FCommandLine::Get(), TEXT("OctreeMin="), tempVal)) tempVal = .1; + OctreeMin = (tempVal*100); + if (!FParse::Value(FCommandLine::Get(), TEXT("OctreeMax="), tempVal)) tempVal = 5; + OctreeMax = (tempVal*100); + + // Calculate where/how big is biggest octree + EnvCenter = (EnvMax + EnvMin) / 2; + OctreeRoot = (EnvMax - EnvMin).GetAbsMax(); + + // Make max/root a multiple of min + tempVal = OctreeMin; + while(tempVal < OctreeMax){ + tempVal *= 2; + } + OctreeMax = tempVal; + while(tempVal < OctreeRoot){ + tempVal *= 2; + } + OctreeRoot = tempVal; + UE_LOG(LogHolodeck, Log, TEXT("Octree:: OctreeMin: %f, OctreeMax: %f, OctreeRoot: %f"), OctreeMin, OctreeMax, OctreeRoot); + + // Load material lookup table + FString filePath = FPaths::ProjectDir() + "../../materials.csv"; + TArray lines; + FFileHelper::LoadANSITextFileToStrings(*filePath, NULL, lines); + for (int i = 1; i < lines.Num(); i++) + { + // Split line into elements + TArray stringArray = {}; + lines[i].ParseIntoArray(stringArray, TEXT(","), false); + + // Put elements into lookup table + FString key = stringArray[0]; + if(stringArray.Num() == 3){ + // density, speed of sound + float z = FCString::Atof(*stringArray[1]) * FCString::Atof(*stringArray[2]); + materials.Add(key, z); + } + } +} + +Octree* Octree::makeEnvOctreeRoot(){ + // Get caching/loading location + FString filePath = FPaths::ProjectDir() + "Octrees/" + World->GetMapName(); + filePath += "/min" + FString::FromInt(OctreeMin) + "_max" + FString::FromInt(OctreeMax); + FString rootFile = filePath + "/" + "roots.json"; + + // load + Octree* root = new Octree(EnvCenter, OctreeRoot, rootFile); + root->makeTill = Octree::OctreeMax; + root->load(); + + // set filename/makeTill for all OctreeMax nodes + std::function fix; + fix = [&filePath, &fix](Octree* tree){ + if(tree->size == Octree::OctreeMax){ + tree->makeTill = Octree::OctreeMin; + tree->file = filePath + "/" + FString::FromInt((int)tree->loc.X) + "_" + + FString::FromInt((int)tree->loc.Y) + "_" + + FString::FromInt((int)tree->loc.Z) + ".json"; + } + else{ + for(Octree* l : tree->leaves){ + fix(l); + } + } + }; + fix(root); + + UE_LOG(LogHolodeck, Log, TEXT("Octree::Made Octree root")); + + return root; +} + +Octree* Octree::makeOctree(FVector center, float octreeSize, float octreeMin, FString actorName){ + FHitResult hit = FHitResult(); + bool occup; + if(octreeSize == Octree::OctreeMin || actorName != ""){ + occup = World->SweepSingleByChannel(hit, center, center+FVector(0.01, 0.01, 0.01), FQuat::Identity, ECollisionChannel::ECC_WorldStatic, FCollisionShape::MakeBox(FVector(octreeSize/2)), params); + } + else{ + occup = World->OverlapBlockingTestByChannel(center, FQuat::Identity, ECollisionChannel::ECC_WorldStatic, FCollisionShape::MakeBox(FVector(octreeSize/2)), params); + } + + // if we're making for an actor, make sure we're hitting it and not something else + if(occup && actorName != "" && actorName != hit.GetActor()->GetName()){ + occup = false; + } + + // if it's occupied + if(occup){ + // Check if it's full + bool full = true; + // check to see if each corner is overlapping + float distToCorner = octreeSize/2 - cornerSize; + for(FVector side : sides){ + if(!full) break; + full = World->OverlapBlockingTestByChannel(center+(side*distToCorner), FQuat::Identity, ECollisionChannel::ECC_WorldStatic, FCollisionShape::MakeBox(FVector(cornerSize)), params); + } + for(FVector corner : corners){ + if(!full) break; + full = World->OverlapBlockingTestByChannel(center+(corner*distToCorner), FQuat::Identity, ECollisionChannel::ECC_WorldStatic, FCollisionShape::MakeBox(FVector(cornerSize)), params); + } + + if(!full){ + // make a tree to insert + Octree* child = new Octree(center, octreeSize); + + // if it still needs to be broken down, iterate through corners + if(octreeSize > octreeMin){ + for(FVector off : corners){ + Octree* l = makeOctree(center+(off*octreeSize/4), octreeSize/2, octreeMin, actorName); + if(l) child->leaves.Add(l); + } + } + + // if it's all the way broken down, save the normal + else if(octreeSize == Octree::OctreeMin){ + child->normal = hit.Normal; + + // Get material (there is tons of these!) + child->fillMaterialProperties(getMaterialName(hit)); + + // clean normal + if(isnan(child->normal.X)) child->normal.X = sign(child->normal.X); + if(isnan(child->normal.Y)) child->normal.Y = sign(child->normal.Y); + if(isnan(child->normal.Z)) child->normal.Z = sign(child->normal.Z); + if(hit.Normal.ContainsNaN()){ + UE_LOG(LogHolodeck, Warning, TEXT("Found position: %s"), *child->loc.ToString()); + UE_LOG(LogHolodeck, Warning, TEXT("Found nan: %s"), *child->normal.ToString()); + } + // DrawDebugLine(World, center, center+hit.Normal*OctreeMin/2, FColor::Blue, true, 100, ECC_WorldStatic, 1.f); + // DrawDebugBox(World, center, FVector(octreeSize/2), FColor::Green, true, 2, ECC_WorldStatic, 5.0f); + } + + return child; + } + } + + // DrawDebugBox(World, center, FVector(octreeSize/2), FColor::Red, true, 2, ECC_WorldStatic, 5.0f); + return nullptr; +} + +int Octree::numLeaves(){ + if(leaves.Num()==0){ + return 1; + } + else{ + int num = 1; + for(Octree* leaf : leaves){ + num += leaf->numLeaves(); + } + return num; + } +} + +void Octree::toJson(){ + // make directory + FFileManagerGeneric().MakeDirectory(*FPaths::GetPath(file), true); + + // calculate buffer size and make writer + int num = numLeaves()*100; + char* buffer = new char[num](); + gason::JSonBuilder doc(buffer, num-1); + + // fill in buffer + toJson(doc); + + if( doc.isBufferAdequate() ){ + // String to file + FILE* fp = fopen(TCHAR_TO_ANSI(*file), "w+t"); + fwrite(buffer, strlen(buffer), 1, fp); + fclose(fp); + } + else{ + UE_LOG(LogHolodeck, Warning, TEXT("Octree: The buffer is too small and the output json for file %s is not valid."), *file); + } + + delete[] buffer; +} + +void Octree::toJson(gason::JSonBuilder& doc){ + doc.startObject() + .startArray("p") + .addValue((int)loc[0]) + .addValue((int)loc[1]) + .addValue((int)loc[2]) + .endArray(); + + if(leaves.Num() != 0){ + doc.startArray("l"); + for(Octree* l : leaves){ + l->toJson(doc); + } + doc.endArray(); + } + if(size == OctreeMin){ + doc.startArray("n") + .addValue(normal[0]) + .addValue(normal[1]) + .addValue(normal[2]) + .endArray() + .addValue("m", TCHAR_TO_ANSI(*material)); + } + + doc.endObject(); +} + +void Octree::load(){ + // if it's not already loaded + if(leaves.Num() == 0){ + // if it's been saved as a json, load it + if(FPaths::FileExists(file)){ + // UE_LOG(LogHolodeck, Log, TEXT("Loading Octree %s"), *file); + // load file to a string + gason::JsonAllocator allocator; + std::ifstream t(TCHAR_TO_ANSI(*file)); + std::string str((std::istreambuf_iterator(t)), + std::istreambuf_iterator()); + char* source = &str[0]; + + // process json + char* endptr; + gason::JsonValue json; + int status = gason::jsonParse(source, &endptr, &json, allocator); + + // load in leaves + for(gason::JsonNode* o : json){ + if(o->key[0] == 'l'){ + for(gason::JsonNode* l : o->value){ + loadJson(l->value, leaves, size/2); + } + } + } + } + + // Otherwise build it & save for later + else{ + // UE_LOG(LogHolodeck, Log, TEXT("Making Octree %s"), *file); + for(FVector off : corners){ + Octree* l = makeOctree(loc+(off*size/4), size/2, makeTill); + if(l) leaves.Add(l); + } + toJson(); + } + + } +} + +void Octree::loadJson(gason::JsonValue& json, TArray& parent, float size){ + Octree* child = new Octree; + for(gason::JsonNode* o : json){ + if(o->key[0] == 'p'){ + gason::JsonNode* arr = o->value.toNode(); + child->loc = FVector(arr->value.toNumber(), arr->next->value.toNumber(), arr->next->next->value.toNumber()); + } + if(o->key[0] == 'l'){ + for(gason::JsonNode* l : o->value){ + loadJson(l->value, child->leaves, size/2); + } + } + if(o->key[0] == 'n'){ + gason::JsonNode* arr = o->value.toNode(); + child->normal = FVector(arr->value.toNumber(), arr->next->value.toNumber(), arr->next->next->value.toNumber()); + } + if(o->key[0] == 'm'){ + child->fillMaterialProperties( FString(o->value.toString()) ); + } + } + child->size = size; + parent.Add(child); +} + +void Octree::unload(){ + if(!isAgent && leaves.Num() != 0){ + // if we need to unload children + if(size > Octree::OctreeMax){ + for(Octree* leaf : leaves) leaf->unload(); + } + + // if we need to unload this one + else if(size == Octree::OctreeMax){ + // UE_LOG(LogHolodeck, Log, TEXT("Unloading Octree %s"), *file); + for(Octree* leaf : leaves) delete leaf; + leaves.Reset(); + } + } +} + +void Octree::fillMaterialProperties(FString mat){ + material = mat; + float matProp; + bool found = materials.Find(material, matProp); + if(!found){ + UE_LOG(LogHolodeck, Warning, TEXT("Missing material information for %s, adding in blank row to csv"), *this->material); + + // Add default line to material file to fill in later + FString filePath = FPaths::ProjectDir() + "../../materials.csv"; + FString line = "\n" + material + ", 10000, 10000"; + FFileHelper::SaveStringToFile(line, *filePath, FFileHelper::EEncodingOptions::AutoDetect, &IFileManager::Get(), EFileWrite::FILEWRITE_Append); + + // Default to something really high to get full reflection for this time + z = 10000*10000; + materials.Add(material, z); + } + else{ + z = matProp; + } +} + +FString Octree::getMaterialName(FHitResult hit){ + // Get staticmesh material + UMaterialInterface* mat = hit.GetComponent()->GetMaterial(hit.ElementIndex); + if(mat != nullptr){ + return mat->GetFName().ToString(); + } + + // If not staticmesh, get landscape material + AActor* actor = hit.GetActor(); + ALandscapeProxy* landscape = reinterpret_cast(actor); + mat = landscape->LandscapeMaterial; + + if(mat != nullptr){ + return mat->GetFName().ToString(); + } + + // If we have extra issues getting material + UE_LOG(LogHolodeck, Warning, TEXT("Couldn't get material name for an octree leaf, putting as MaterialNotFound")); + return "MaterialNotFound"; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/RenderRequest.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/RenderRequest.cpp new file mode 100644 index 0000000000..1865ebafbc --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/RenderRequest.cpp @@ -0,0 +1,50 @@ +#include "Holodeck.h" +#include "RenderRequest.h" + +DEFINE_STAT(STAT_CameraExecuteTask); +DEFINE_STAT(STAT_CameraExecuteTask_ReadSurfaceData); +DEFINE_STAT(STAT_CameraExecuteTask_Memcpy); + +void FRenderRequest::RetrievePixels(FColor* PixelBuffer, UTextureRenderTarget2D* Texture) { + this->Buffer = PixelBuffer; + this->TargetTexture = Texture; + CheckNotBlockedOnRenderThread(); // not the issue + + // Queue up the task of rendering the scene in the render thread + TGraphTask::CreateTask().ConstructAndDispatchWhenReady(*this); +} + +void FRenderRequest::ExecuteTask() +{ + SCOPE_CYCLE_COUNTER(STAT_CameraExecuteTask); + + + TArray SurfaceData; + FRHICommandListImmediate& RHICmdList = GetImmediateCommandList_ForRenderCommand(); + FTextureRenderTargetResource* rt_resource = TargetTexture->GetRenderTargetResource(); + + if (rt_resource != nullptr) { + const FTexture2DRHIRef& rhi_texture = rt_resource->GetRenderTargetTexture(); + FIntPoint size; + FReadSurfaceDataFlags flags(RCM_UNorm, CubeFace_MAX); + flags.SetLinearToGamma(false); + + { + SCOPE_CYCLE_COUNTER(STAT_CameraExecuteTask_ReadSurfaceData); + // This next call is slow! Significant impact on the frame time (~8ms) + RHICmdList.ReadSurfaceData( + rhi_texture, + FIntRect(0, 0, TargetTexture->SizeX, TargetTexture->SizeY), + SurfaceData, + flags); + } + + { + SCOPE_CYCLE_COUNTER(STAT_CameraExecuteTask_Memcpy); + FMemory::Memcpy(this->Buffer, &SurfaceData[0], SurfaceData.Num() * sizeof(FColor)); // this line isn't the problem + } + + } +} + +FRenderRequest::~FRenderRequest() {}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/SimplePID.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/SimplePID.cpp new file mode 100644 index 0000000000..64812420b9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Private/SimplePID.cpp @@ -0,0 +1,101 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "SimplePID.h" + +SimplePID::SimplePID() { + Integrator = 0.0f; + Differentiator = 0.0f; + LastError = 0.0f; + LastState = 0.0f; + Tau = 0.0f; // Connor: What does tau represent? +} + +SimplePID::SimplePID(const float P, const float I, const float D) + : P(P), I(I), D(D), Tau(0.05) { + Integrator = 0.0f; + Differentiator = 0.0f; + LastError = 0.0f; + LastState = 0.0f; +} + +float SimplePID::ComputePID(float Desired, float Current, float Delta) { + // Get the error + float Error = Desired - Current; + + // If dt is 0 or the error is massive, that's a probelm + if (Delta == 0.0 || MyAbs(Error) > 9999999) { + LastError = Error; + LastState = Current; + return 0.0; + } + + Integrator += Delta / 2.0 * (Error + LastError); + + // Derivative + // Noise reduction (See "Small Unmanned Aircraft". Chapter 6. Slide 31/33) + // d/dx w.r.t. error:: differentiator_ = (2*tau_ - dt)/(2*tau_ + dt)*differentiator_ + 2/(2*tau_ + dt)*(error - last_error_); + if (Delta > 0.0) + Differentiator = (2.0 * Tau - Delta) / (2.0 * Tau + Delta) * Differentiator + 2.0 / (2.0 * Tau + Delta) * (Current - LastState); + + LastError = Error; + LastState = Current; + + // Note the negative der. term. This is because now the differentiator is in the feedback loop rather than the forward loop + return P * Error + I * Integrator - D * Differentiator; +} + +float SimplePID::ComputePIDDirect(float XC, float X, float XDot, float Delta) { + //XC is the desired, X is the current + float Error = XC - X; + + if (Delta == 0 || MyAbs(Error) > 9999999) { + LastError = Error; + LastState = X; + } + + Integrator += Delta / 2.0 * (Error + LastError); + + LastError = Error; + LastState = X; + return P * Error + I * Integrator - D * XDot; +} + +float SimplePID::ComputePIDDirect(float XC, float X, float XDot, float Delta, bool bIsAngle) { + // XC is the desired, X is the current + float Error = XC - X; + + if (bIsAngle) { + if (Error >= PI) + Error -= (2.0 * PI); + else if (Error <= -PI) + Error += (2 * PI); + } + + if (Delta == 0 || MyAbs(Error) > 9999999) { + LastError = Error; + LastState = X; + } + + Integrator += Delta / 2.0 * (Error + LastError); + LastError = Error; + LastState = X; + + return P * Error + I * Integrator - D * XDot; +} + +void SimplePID::SetGains(const float NewP, const float NewI, const float NewD) { + P = NewP; + I = NewI; + D = NewD; + + Tau = 0.05; // time for force to go from 0 to 63.2% of maximum desired force + return; +} + +float SimplePID::MyAbs(float x) { + if (x < 0) + return -x; + else + return x; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Conversion.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Conversion.h new file mode 100644 index 0000000000..40b21ce926 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Conversion.h @@ -0,0 +1,25 @@ +#pragma once + +#include "Holodeck.h" +#include "Kismet/KismetMathLibrary.h" + +const float UEUnitsPerMeter = 100.0; +const float UEUnitsPerMeterSquared = 10000; + +enum ConvertType {UEToClient, ClientToUE, NoScale}; + +FRotator RPYToRotator(float Roll, float Pitch, float Yaw); + +FVector RotatorToRPY(FRotator Rot); + +FVector ConvertLinearVector(FVector Vector, ConvertType Type); + +FVector ConvertAngularVector(FVector Vector, ConvertType Type); + +FRotator ConvertAngularVector(FRotator Rotator, ConvertType); + +FVector ConvertTorque(FVector Vector, ConvertType Type); + +float ConvertClientDistanceToUnreal(float client); + +float ConvertUnrealDistanceToClient(float unreal); \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/HolodeckViewportClient.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/HolodeckViewportClient.h new file mode 100644 index 0000000000..0f444291c1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/HolodeckViewportClient.h @@ -0,0 +1,63 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "Engine/GameViewportClient.h" +#include "HolodeckViewportClient.generated.h" + +class UHolodeckCamera; //Must forward declare instead of include to avoid cyclic dependency. + +/** + * UHolodeckViewportClient + * Handles capturing the main player camera image data. + * Must be set in the editor to be the viewport client. + * + * THIS MUST BE IN DefaultEngine.ini + * GameViewportClientClassName = /Script/Holodeck.HolodeckViewportClient + */ +UCLASS() +class HOLODECK_API UHolodeckViewportClient : public UGameViewportClient{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UHolodeckViewportClient(const FObjectInitializer& PCIP); + + /** + * Draw + * Calls the parent draw function, and captures the pixels. + */ + void Draw(FViewport* Viewport, FCanvas* SceneCanvas) override; + + /** + * SetBuffer + * Sets the buffer to capture pixels to. + * @param NewBuffer the buffer to save the pixel data to. + */ + void SetBuffer(void* NewBuffer); + + /** + * BufferIsSet + * Gets whether the buffer has been set. + * @return true if the buffer has been set. + */ + bool BufferIsSet() { return Buffer != nullptr; }; + +private: + /** + * HolodeckTakeScreenShot + * Takes a screenshot of the main player camera. + * Must only be called from inside the draw function! + */ + void HolodeckTakeScreenShot(); + + //void ClearNullCameras(); + + FColor* Buffer; + FVector2D ViewportSize; + TMap Cameras; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateNormal.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateNormal.h new file mode 100644 index 0000000000..7470b0b30c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateNormal.h @@ -0,0 +1,212 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once +#include +#include + +/** + * Sample from a mean 0 multivariate normal distribution + * with covariance as specified in the constructor + */ +template +class HOLODECK_API MultivariateNormal +{ +static_assert(N > 0, "MVN: N must be > 0"); + +public: + MultivariateNormal(){} + ~MultivariateNormal(){} + + /* + * Set up covariance from a std deviation + * Can be a float => covariance is diagonal with that #^2 + * Can be a 3-array => covariance is diagonal with that vector^2 + * Can be a FJsonValue 3-array => covariance is diagonal with that vector^2 + */ + void initSigma(float sigma){ + for(int i=0;i sigma){ + for(int i=0;i> sigma){ + verifyf(sigma.Num() == N, TEXT("Sigma has size %d and should be %d"), sigma.Num(), N); + + for(int i=0;iAsNumber(); + } + uncertain = true; + } + + + /* + * Set up covariance from a covariance + * Can be a float => covariance is diagonal with that # + * Can be a 3-array => covariance is diagonal with that vector + * Can be 3x3-array => covariance is that array + * Can be a FJsonValue 3-array => covariance is diagonal with that vector + * Can be a FJsonValue 3x3-array => covariance is that array + */ + void initCov(float cov){ + for(int i=0;i cov){ + for(int i=0;i,N> cov){ + sqrtCov = cov; + + bool success = Cholesky(sqrtCov); + if(success){ + uncertain = true; + } + else{ + UE_LOG(LogHolodeck, Warning, TEXT("MVN: Encountered a non-positive definite covariance")); + } + } + void initCov(TArray> cov){ + verifyf(cov.Num() == N, TEXT("Cov has size %d and should be %d"), cov.Num(), N); + + double temp; + bool is2D = !(cov[0]->TryGetNumber(temp)); + + if(is2D){ + std::array,N> parsed; + for(int i=0;i> row = cov[i]->AsArray(); + verifyf(row.Num() == N, TEXT("Cov Row has size %d and should be %d"), cov.Num(), N); + for(int j=0;jAsNumber(); + } + } + initCov(parsed); + } + else{ + for(int i=0;iAsNumber()); + } + } + uncertain = true; + } + + + /* + * Different ways to sample, with different return types + * Can return as std::array, TArray, FVector (requires N=3), or float (requires N=1) + * All functions draw on sampleArray. If cov hasn't been set, returns 0s + */ + std::array sampleArray(){ + std::array sam; + std::array result = {0}; + + if(uncertain){ + // sample from N(0,1); + for(int i=0;i sampleTArray(){ + // sample + std::array sample = sampleArray(); + + // put into TArray + TArray result; + result.Append(sample.data(), N); + + return result; + } + FVector sampleFVector(){ + verifyf(N == 3, TEXT("Can't use MVN size %d with FVector samples"), N); + + // sample + std::array sample = sampleArray(); + + // put into TArray + FVector result = FVector(sample[0], sample[1], sample[2]); + + return result; + } + float sampleFloat(){ + verifyf(N == 1, TEXT("Can't use MVN size %d with float samples"), N); + + // sample + std::array sample = sampleArray(); + + return sample[0]; + } + float sampleRayleigh(){ + // Samples from a rayleigh distribution + verifyf(N == 1, TEXT("Can't use MVN size %d with Rayleigh Noise"), N); + + // sample + float x = sampleFloat(); + float y = sampleFloat(); + + return sqrt(x*x + y*y); + } + + /* + * Computes cholesky decomp in place + * returns false if matrix isn't positive definite + */ + static bool Cholesky(std::array,N>& A){ + for(int32 I=0;I= 0; --K){ + Sum -= A[I][K] * A[J][K]; + } + if (I == J){ + if (Sum <= 0){ + // Not positive definite (rounding?) + return false; + } + A[I][J] = FMath::Sqrt(Sum); + } + else{ + A[J][I] = Sum / A[I][I]; + } + } + } + + for(int32 I=0;I,N> getSqrtCov(){ return sqrtCov; } + bool isUncertain(){ return uncertain;} + +private: + bool uncertain = false; + std::array,N> sqrtCov = {{{{0}}}}; + std::normal_distribution dist{0.0f, 1.0f}; + std::random_device rd{}; + std::mt19937 gen{ rd() }; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateUniform.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateUniform.h new file mode 100644 index 0000000000..eefb215d77 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/MultivariateUniform.h @@ -0,0 +1,128 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once +#include +#include +#include + +/** + * Sample from a min 0 multivariate uniform distribution + * with maximum as specified in the constructor + * + * TODO: This library has NOT been fully tested. Beware before using! + */ +template +class HOLODECK_API MultivariateUniform +{ +static_assert(N > 0, "UNIFORM: N must be > 0"); + +public: + MultivariateUniform(){} + ~MultivariateUniform(){} + + /* + * Set up covariance from a std deviation + * Can be a float => covariance is diagonal with that #^2 + * Can be a 3-array => covariance is diagonal with that vector^2 + * Can be a FJsonValue 3-array => covariance is diagonal with that vector^2 + */ + void initBounds(float max_){ + for(int i=0;i max_){ + for(int i=0;i> max_){ + verifyf(max_.Num() == N, TEXT("Sigma has size %d and should be %d"), max_.Num(), N); + + for(int i=0;iAsNumber(); + if(max_[i] != 0) uncertain = true; + } + } + + /* + * Different ways to sample, with different return types + * Can return as std::array, TArray, FVector (requires N=3), or float (requires N=1) + * All functions draw on sampleArray. If cov hasn't been set, returns 0s + */ + std::array sampleArray(){ + std::array sam; + std::array result = {0}; + + if(uncertain){ + // sample from N(0,1); + for(int i=0;i sampleTArray(){ + // sample + std::array sample = sampleArray(); + + // put into TArray + TArray result; + result.Append(sample.data(), N); + + return result; + } + FVector sampleFVector(){ + verifyf(N == 3, TEXT("Can't use MVN size %d with FVector samples"), N); + + // sample + std::array sample = sampleArray(); + + // put into TArray + FVector result = FVector(sample[0], sample[1], sample[2]); + + return result; + } + float sampleFloat(){ + verifyf(N == 1, TEXT("Can't use MVN size %d with float samples"), N); + + // sample + std::array sample = sampleArray(); + + return sample[0]; + } + float sampleExponential(){ + // Samples from a exponential distribution using max as the scale parameter + verifyf(N == 1, TEXT("Can't use MVN size %d with Exponential Noise"), N); + + // sample + if(uncertain){ + float x = dist(gen); + // https://en.wikipedia.org/wiki/Exponential_distribution#Generating_exponential_variates + return -max[0]*std::log(x); + } + else{ + return 0; + } + } + float exponentialPDF(float x){ + if(uncertain) return std::exp(-x/max[0]) / max[0]; + else return 1; + } + float exponentialScaledPDF(float x){ + if(uncertain) return std::exp(-x/max[0]); + else return 1; + } + + bool isUncertain(){ return uncertain;} + +private: + bool uncertain = false; + std::array max = {{0}}; + std::uniform_real_distribution dist{0.0f, 1.0f}; + std::random_device rd{}; + std::mt19937 gen{ rd() }; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h new file mode 100644 index 0000000000..1cd29d6c14 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/Octree.h @@ -0,0 +1,119 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "Engine/World.h" +#include "Misc/FileHelper.h" +#include "HAL/FileManagerGeneric.h" +#include "Containers/Map.h" +#include "Containers/DiscardableKeyValueCache.h" +#include "DrawDebugHelpers.h" +#include "LandscapeProxy.h" + +#include "Conversion.h" +#include "gason.h" +#include "jsonbuilder.h" +#include +#include +#include +#include + +class Octree +{ + private: + // Globals used for calculations + static TArray corners; + static TArray sides; + static FCollisionQueryParams params; + static float cornerSize; + static FVector EnvMin; + static FVector EnvMax; + static UWorld* World; + static TDiscardableKeyValueCache materials; + + static FVector EnvCenter; + + static void loadJson(gason::JsonValue& json, TArray& parent, float size); + void toJson(gason::JSonBuilder& doc); + + static FCollisionQueryParams init_params(){ + FCollisionQueryParams p; + p.bTraceComplex = false; + p.TraceTag = ""; + // p.bFindInitialOverlaps = true; + // p.bReturnPhysicalMaterial = true; + // p.bReturnFaceIndex = true; + return p; + } + + static FString getMaterialName(FHitResult hit); + void fillMaterialProperties(FString mat); + + public: + static float OctreeRoot; + static float OctreeMax; + static float OctreeMin; + + Octree(){}; + Octree(FVector loc, float size, FString file="") : size(size), loc(loc), file(file) {}; + ~Octree(){ + for(Octree* leaf : leaves){ + delete leaf; + } + leaves.Reset(); + } + + // Used to setup octree globals + static void initOctree(UWorld* w); + + // Figures out where octree roots are + static Octree* makeEnvOctreeRoot(); + + // iterative constructs octree + static Octree* makeOctree(FVector center, float octreeSize, float octreeMin, FString actorName=""); + + void unload(); + void load(); + + // helpers for saving + void toJson(); + + // ignore actors + static void ignoreActor(const AActor * InIgnoreActor){ + params.AddIgnoredActor(InIgnoreActor); + } + static void resetParams(){ params = init_params(); } + + int numLeaves(); + + // Used to check if it's a dynamic octree for an agent + bool isAgent = false; + + // Given to all + float size; + FVector loc; + + // Given to octree roots that have been saved/loaded from file + FString file; + float makeTill; + + // Given to each non-leaf + TArray leaves; + + // Given to each leaf + FVector normal; + FString material; + // impedance + float z = 1.0f; + + // Used during computations + // Value of Range, Elevation, and Azimuth in that order (in cm/degrees/degrees). + FVector locSpherical; + FVector normalImpact; + // Index of Range, Elevation, and Azimuth in that order. + FIntVector idx; + // Holds cos of angle, and value to put in + float cos; + float val; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/RenderRequest.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/RenderRequest.h new file mode 100644 index 0000000000..1bff19b8f7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/RenderRequest.h @@ -0,0 +1,42 @@ +#pragma once + +#include "Holodeck.h" + +#include "CoreMinimal.h" +#include "Engine/TextureRenderTarget2D.h" + +// Adapted from Microsoft Airsim Open Source Project +// https://github.com/Microsoft/AirSim/blob/7bddd5857791f9c164e8eba80c229f199c0babf8/Unreal/Plugins/AirSim/Source/RenderRequest.h + +DECLARE_STATS_GROUP(TEXT("RenderRequest"), STATGROUP_RenderRequest, STATCAT_Advanced); +DECLARE_CYCLE_STAT_EXTERN(TEXT("CameraExecuteTask"), STAT_CameraExecuteTask, STATGROUP_RenderRequest, ); +DECLARE_CYCLE_STAT_EXTERN(TEXT("CameraExecuteTask_ReadSurfaceData"), STAT_CameraExecuteTask_ReadSurfaceData, STATGROUP_RenderRequest, ); +DECLARE_CYCLE_STAT_EXTERN(TEXT("CameraExecuteTask_Memcpy"), STAT_CameraExecuteTask_Memcpy, STATGROUP_RenderRequest, ); + +class FRenderRequest : public FRenderCommand +{ + private: + FColor* Buffer; + UTextureRenderTarget2D* TargetTexture; + + public: + virtual ~FRenderRequest(); + + /** + * Retrieves the rendered texture from the GPU without flushing the GPU like ReadPixels() does. + */ + virtual void RetrievePixels(FColor* Buffer, UTextureRenderTarget2D* TargetTexture); + + void DoTask(ENamedThreads::Type CurrentThread, const FGraphEventRef& MyCompletionGraphEvent) + { + ExecuteTask(); + } + + FORCEINLINE TStatId GetStatId() const + { + RETURN_QUICK_DECLARE_CYCLE_STAT(RenderRequest, STATGROUP_RenderThreadCommands); + } + + void ExecuteTask(); + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/SimplePID.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/SimplePID.h new file mode 100644 index 0000000000..61d80952ca --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/General/Public/SimplePID.h @@ -0,0 +1,80 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +/** + * A simple PID controller. + */ +class HOLODECK_API SimplePID { +public: + /** + * Default Constructor. + */ + SimplePID(); + + /** + * Full Constructor. + * Constructs the PID controller with values for P, I, D, and Tau. + */ + SimplePID(const float P, const float I, const float D); + + /** + * Default Destructor. + */ + ~SimplePID() = default; + + /** + * ComputePID + * Computes the control + * @param Desired the desired value + * @param Current the current value + * @param Delta the delta time + * @return the PID + */ + float ComputePID(float Desired, float Current, float Delta); + + /** + * ComputePIDDirect + * Computes the PID directly? + * @param XC desired + * @param X current + * @param XDot the gradient (angular velocity?) + * @param Delta the change in time + * @return force or torque + */ + float ComputePIDDirect(float XC, float X, float XDot, float Delta); + + + /** + * ComputePIDDirect + * Computes the PID directly? + * @param XC desired + * @param X current + * @param XDot the gradient (angular velocity?) + * @param Delta the change in time + * @param bIsAngle ? + * @return Force or torque + */ + float ComputePIDDirect(float XC, float X, float XDot, float Delta, bool bIsAngle); + + + /** + * SetGains + * + * Used for late initialization or a redo + */ + void SetGains(const float P, const float I, const float D); + +private: + float Integrator; + float Differentiator; + float LastError; + float LastState; + + float P; + float I; + float D; + float Tau; + + float MyAbs(float I); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.Build.cs b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.Build.cs new file mode 100644 index 0000000000..1409728057 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.Build.cs @@ -0,0 +1,27 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +using UnrealBuildTool; + +public class Holodeck : ModuleRules +{ + public Holodeck(ReadOnlyTargetRules Target) : base(Target) + { + PrivatePCHHeaderFile = "Holodeck.h"; + PrivateIncludePaths.AddRange(new [] { + "Holodeck/Agents/Public", + "Holodeck/General/Public", + "Holodeck/Sensors/Public", + "Holodeck/Utils/Public", + "Holodeck/HolodeckCore/Public", + "Holodeck/ClientCommands/Public", + "Holodeck/Tasks/Public" + }); + + PublicDependencyModuleNames.AddRange(new [] {"ApplicationCore", "Core", "CoreUObject", "Engine", "InputCore", "AIModule", "SlateCore", "Slate", "PhysX", "APEX", "Json", "JsonUtilities", "RenderCore", "RHI", "Landscape" }); + +#if PLATFORM_LINUX + PublicDependencyModuleNames.AddRange(new [] { "rt", "pthread" }; + //TARGET_LINK_LIBRARIES(UHolodeckServer rt pthread) +#endif + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp new file mode 100644 index 0000000000..9f3d5d19cf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.cpp @@ -0,0 +1,7 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" + +IMPLEMENT_PRIMARY_GAME_MODULE( FDefaultGameModuleImpl, Holodeck, "Holodeck" ); + +DEFINE_LOG_CATEGORY(LogHolodeck); \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.h new file mode 100644 index 0000000000..6d2a75d7a9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Holodeck.h @@ -0,0 +1,7 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Engine.h" + +DECLARE_LOG_CATEGORY_EXTERN(LogHolodeck, Log, All); \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckAgent.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckAgent.cpp new file mode 100644 index 0000000000..ed3cdcf8f1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckAgent.cpp @@ -0,0 +1,124 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckAgent.h" +#include "HolodeckSensor.h" + +AHolodeckAgent::AHolodeckAgent() { + PrimaryActorTick.bCanEverTick = true; + PrimaryActorTick.TickGroup = TG_PrePhysics; //The tick function will we called before any physics simulation. + AddTickPrerequisiteActor(GetController()); //The agent's controller will always tick before the agent. +} + +void AHolodeckAgent::BeginPlay(){ + Super::BeginPlay(); +} + +void AHolodeckAgent::InitializeAgent() { + + UE_LOG(LogHolodeck, Log, TEXT("Initializing HolodeckAgent %s"), *AgentName); + if (!InitializeController()) + UE_LOG(LogHolodeck, Warning, TEXT("Couldn't initialize HolodeckPawnController for HolodeckAgent.")); + + //Need to initialize this so that collision events will work (OnActorHit won't be called without it) + //This is needed specifically for the collision sensor. + if (UPrimitiveComponent* PrimitiveComponent = Cast(RootComponent)) { + PrimitiveComponent->SetNotifyRigidBodyCollision(true); + UE_LOG(LogHolodeck, Log, TEXT("HolodeckAgent collision events enabled")); + } else { + UE_LOG(LogHolodeck, Warning, TEXT("HolodeckAgent unable to get UPrimitiveComponent. Collision events disabled.")); + } + + Instance = static_cast(GetGameInstance()); + Server = Instance->GetServer(); + + UE_LOG(LogHolodeck, Log, TEXT("Adding Agent %s to Server"), *AgentName); + if (Server == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("Agent could not find server...")); + } else { + Server->AgentMap.Add(*AgentName, this); + } + + // Initialize Sensors + TArray Sensors; + this->GetComponents(UHolodeckSensor::StaticClass(), Sensors, false); + + for (auto& ActorSensor : Sensors) { + UHolodeckSensor* Sensor = Cast(ActorSensor); + Sensor->SetAgentAndController(HolodeckController, AgentName); + Sensor->InitializeSensor(); + this->SensorMap.Add(Sensor->SensorName, Sensor); + } +} + +void AHolodeckAgent::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); +} + +bool AHolodeckAgent::Teleport(const FVector& NewLocation, const FRotator& NewRotation) { + UE_LOG(LogHolodeck, Log, TEXT("Attempting to teleport HolodeckAgent %s"), *AgentName); + FHitResult DummyHitResult; + bool bWasSuccessful = this->K2_SetActorLocationAndRotation( + NewLocation, + NewRotation, + false, //will not be blocked by object in between current and new location. + DummyHitResult, //this object is where the hit result is reported, if teleport can be blocked by objects in between. + true //the object will retain its momentum(otherwise the android could not be teleported). + ); + + if (bWasSuccessful) { + UE_LOG(LogHolodeck, Log, TEXT("HolodeckAgent %s teleported successfully"), *AgentName); + } else { + UE_LOG(LogHolodeck, Warning, TEXT("HolodeckAgent %s did not teleport successfully"), *AgentName); + } + + return bWasSuccessful; +} + +bool AHolodeckAgent::Teleport(const FVector& NewLocation){ + FRotator DefaultRotation = this->GetActorRotation(); + return Teleport(NewLocation, DefaultRotation); +} + +bool AHolodeckAgent::SetState(const FVector& NewLocation, const FRotator& NewRotation, const FVector& NewVelocity, const FVector& NewAngVelocity) { + + FHitResult HitResult; + bool bWasSuccessful = this->K2_SetActorLocationAndRotation( + NewLocation, + NewRotation, + true, // will sweep and be blocked by an object in the path + HitResult, //this object is where the hit result is reported, if teleport can be blocked by objects in between. + false + ); + + UPrimitiveComponent* RootComp = (UPrimitiveComponent*)this->GetRootComponent(); + + FRotator RotationNow = RootComp->GetComponentRotation(); + FVector AngularVelocityVector = RotationNow.RotateVector(NewAngVelocity); //Rotate from local angles to world angles + + RootComp->SetAllPhysicsLinearVelocity(NewVelocity, false); + RootComp->SetAllPhysicsAngularVelocityInDegrees(AngularVelocityVector, false); + + return bWasSuccessful; +} + +bool AHolodeckAgent::InitializeController() { + UE_LOG(LogHolodeck, Log, TEXT("Attempting to initialize controller for HolodeckAgent")); + + if (AgentName == "") { + UE_LOG(LogHolodeck, Error, TEXT("HolodeckAgent doesn't have a name! Must set HolodeckAgent's name")); + return false; + } + + HolodeckController = static_cast(Controller); + + if (HolodeckController == nullptr) { + UE_LOG(LogHolodeck, Error, TEXT("Couldn't find a HolodeckPawnController for HolodeckAgent")); + return false; + } else { + // We found the controller, so tell it to set up the action buffers. + HolodeckController->AllocateBuffers(AgentName); + UE_LOG(LogHolodeck, Log, TEXT("HolodeckAgent controller setup was successful")); + return true; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckBuoyantAgent.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckBuoyantAgent.cpp new file mode 100644 index 0000000000..a9f1c748cb --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckBuoyantAgent.cpp @@ -0,0 +1,143 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "HolodeckBuoyantAgent.h" + +void AHolodeckBuoyantAgent::InitializeAgent(){ + Super::InitializeAgent(); + + // Get GravityVectority from world + AWorldSettings* WorldSettings = GetWorld()->GetWorldSettings(false, false); + Gravity = WorldSettings->GetGravityZ() / -100; + + // Set Mass + RootMesh->SetMassOverrideInKg("", MassInKG); + // Set COM (have to do some calculation since it sets an offset) + FVector COM_curr = GetActorRotation().UnrotateVector( RootMesh->GetCenterOfMass() - GetActorLocation() ); + RootMesh->SetCenterOfMass( CenterMass + OffsetToOrigin - COM_curr ); + + // Set Bounding Box (if it hasn't been set by hand) + if(BoundingBox.GetExtent() == FVector(0, 0, 0)) + BoundingBox = RootMesh->GetStaticMesh()->GetBoundingBox(); + + // Sample points (if they haven't already been set) + if(SurfacePoints.Num() == 0){ + for(int i=0;iAddForceAtLocation(BuoyantVector, COB_World); + + FVector GravityVector = ConvertLinearVector(FVector(0, 0, -Gravity*MassInKG), ClientToUE); + RootMesh->AddForceAtLocation(GravityVector, RootMesh->GetCenterOfMass()); +} + +void AHolodeckBuoyantAgent::ShowBoundingBox(){ + FVector location = GetActorLocation() + GetActorRotation().RotateVector(OffsetToOrigin + CenterVehicle); + DrawDebugBox(GetWorld(), location, BoundingBox.GetExtent(), GetActorQuat(), FColor::Red, false, 0.05, 0, 1); +} + +void AHolodeckBuoyantAgent::ShowSurfacePoints(){ + FVector ActorLocation = GetActorLocation(); + FRotator ActorRotation = GetActorRotation(); + FVector* points = SurfacePoints.GetData(); + + for(int i=0;iisAgent = true; + octreeGlobal->file = "AGENT"; + + // Convert our global octree to a local one + octreeLocal = cleanOctree(octreeGlobal); + } + else{ + UE_LOG(LogHolodeck, Warning, TEXT("HolodeckBuoyantAgent:: Failed to make Octree")); + } + + } + + return octreeGlobal; +} + +Octree* AHolodeckBuoyantAgent::cleanOctree(Octree* globalFrame){ + Octree* local = new Octree; + local->loc = GetActorRotation().UnrotateVector(globalFrame->loc - GetActorLocation()); + local->normal = GetActorRotation().UnrotateVector(globalFrame->normal); + + for( Octree* tree : globalFrame->leaves){ + Octree* l = cleanOctree(tree); + local->leaves.Add(l); + } + + return local; +} + +void AHolodeckBuoyantAgent::updateOctree(Octree* localFrame, Octree* globalFrame){ + globalFrame->loc = GetActorLocation() + GetActorRotation().RotateVector(localFrame->loc); + globalFrame->normal = GetActorRotation().RotateVector(localFrame->normal); + + for(int i=0;ileaves.Num();i++){ + updateOctree(localFrame->leaves[i], globalFrame->leaves[i]); + } +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckControlScheme.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckControlScheme.cpp new file mode 100644 index 0000000000..6b8bccd387 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckControlScheme.cpp @@ -0,0 +1,18 @@ +#include "Holodeck.h" + +#include "HolodeckControlScheme.h" + + +UHolodeckControlScheme::UHolodeckControlScheme() {} + +UHolodeckControlScheme::UHolodeckControlScheme(const FObjectInitializer& ObjectInitializer) : + Super(ObjectInitializer) {} + +void UHolodeckControlScheme::Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds) { + check(0 && "You must override Execute"); +} + +unsigned int UHolodeckControlScheme::GetControlSchemeSizeInBytes() const { + check(0 && "You must override GetControlSchemeByteSize"); + return 0; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameInstance.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameInstance.cpp new file mode 100644 index 0000000000..b5051b1762 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameInstance.cpp @@ -0,0 +1,38 @@ +// Written by joshgreaves + +#include "Holodeck.h" +#include "HolodeckGameInstance.h" + +void UHolodeckGameInstance::StartServer() { + // HolodeckGameMode should start the server, so it will call this function. + if (Server == nullptr) { + Server = NewObject(); + Server->Start(); + } +} + +UHolodeckServer* UHolodeckGameInstance::GetServer() { + return Server; +} + +UHolodeckGameInstance::UHolodeckGameInstance(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer) { + UE_LOG(LogHolodeck, Log, TEXT("Holodeck Game Instance initialized")); +} + +void UHolodeckGameInstance::Tick(float DeltaTime) { + // HolodeckGameMode will call tick on this. + // The release and acquire is what allows this to work in lock step with the client. + static bool bFirstTime = true; + if (!bFirstTime) Server->Release(); + Server->Acquire(); + bFirstTime = false; +} + +void UHolodeckGameInstance::Init(){ + Super::Init(); + + // TODO: Ensure this code also gets called when a new level is loaded + UWorld* World = GetWorld(); + if (World) + WorldSettings = static_cast(World->GetWorldSettings()); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameMode.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameMode.cpp new file mode 100644 index 0000000000..dc35830ab3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckGameMode.cpp @@ -0,0 +1,83 @@ +// Written by joshgreaves. + +#include "Holodeck.h" +#include "HolodeckGameMode.h" + +const char RESET_KEY[] = "RESET"; +const int RESET_BYTES = 1; + +AHolodeckGameMode::AHolodeckGameMode(const FObjectInitializer& ObjectInitializer) : AGameMode(ObjectInitializer), bHolodeckIsOn(true) { + PrimaryActorTick.bCanEverTick = true; + PrimaryActorTick.TickGroup = TG_PrePhysics; + UE_LOG(LogHolodeck, Log, TEXT("HolodeckGameMode initialized")); +} + +void AHolodeckGameMode::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + + // If !bHolodeckIsOn, then we never got instance or reset signal, + // so we don't need to check bOn here. + if (this->Instance) + this->Instance->Tick(DeltaSeconds); + if (this->CommandCenter) + this->CommandCenter->Tick(DeltaSeconds); + //Check if we should reset, and then reset the level. + if (ResetSignal != nullptr && *ResetSignal) { + UGameplayStatics::OpenLevel(this->Instance, FName(*GetWorld()->GetName()), false); + *ResetSignal = false; + } + +} + +void AHolodeckGameMode::StartPlay() { + UE_LOG(LogHolodeck, Log, TEXT("HolodeckGameMode starting play")); + + // To prevent crashing in standalone games, check the HolodeckOn command is supplied. + // This overrides the bHolodeckIsOn value supplied in the editor. + //if (GetWorld()->WorldType == EWorldType::Game) + // bHolodeckIsOn = FParse::Param(FCommandLine::Get(), TEXT("HolodeckOn")); + + // Make sure Octree is properly initialized + Octree::initOctree(GetWorld()); + + // Cap our tickrate + int FramesPerSec; + if (FParse::Value(FCommandLine::Get(), TEXT("FramesPerSec="), FramesPerSec)) { + FString command = "t.MaxFPS " + FString::FromInt(FramesPerSec); + bool succeeded = GEngine->Exec(GetWorld(), *command); + + if (!succeeded) { + UE_LOG(LogHolodeck, Warning, TEXT("Unable to cap frametrate")); + } + } + + if (bHolodeckIsOn) { + this->Instance = (UHolodeckGameInstance*)(GetGameInstance()); + if (this->Instance) { + this->Instance->StartServer(); + Server = this->Instance->GetServer(); + + RegisterSettings(); + } else { + UE_LOG(LogHolodeck, Warning, TEXT("Game Instance couldn't be found and initialized")); + } + if (this->Server) { + this->CommandCenter = NewObject(); + CommandCenter->Init(Server, this); + } + } + + Super::StartPlay(); +} + +void AHolodeckGameMode::RegisterSettings() { + UE_LOG(LogHolodeck, Log, TEXT("Registering Settings")); + if (Server != nullptr) { + ResetSignal = static_cast(Server->Malloc(RESET_KEY, RESET_BYTES)); + UE_LOG(LogHolodeck, Log, TEXT("Reset signal registered")); + } +} + +void AHolodeckGameMode::LogFatalMessage(const FString& Message) { + UE_LOG(LogHolodeck, Fatal, TEXT("%s"), *Message); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckPawnController.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckPawnController.cpp new file mode 100644 index 0000000000..1810354bbd --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckPawnController.cpp @@ -0,0 +1,169 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckAgent.h" +#include "HolodeckPawnController.h" + +const FString CONTROL_SCHEME_KEY = "control_scheme"; +const FString TELEPORT_FLAG_KEY = "teleport_flag"; +const FString TELEPORT_COMMAND_KEY = "teleport_command"; + + +AHolodeckPawnController::AHolodeckPawnController(const FObjectInitializer& ObjectInitializer) + : AHolodeckPawnControllerInterface(ObjectInitializer) { + PrimaryActorTick.bCanEverTick = true; + PrimaryActorTick.TickGroup = TG_PrePhysics; +} + +AHolodeckPawnController::~AHolodeckPawnController() { } + +void AHolodeckPawnController::BeginPlay() { + Super::BeginPlay(); + AddTickPrerequisiteActor(GetWorld()->GetAuthGameMode()); +} + +void AHolodeckPawnController::OnPossess(APawn* InPawn) { + ControlledAgent = static_cast(InPawn); + + if (ControlledAgent == nullptr) { + UE_LOG(LogHolodeck, Fatal, TEXT("HolodeckPawnController attached to non-HolodeckAgent!")); + } + + ControlledAgent->Controller = this; + + UE_LOG(LogHolodeck, Log, TEXT("Pawn Possessed: %s, Controlled by: %s"), *InPawn->GetHumanReadableName(), *this->GetClass()->GetName()); + UpdateServerInfo(); + + URawControlScheme* RawControlScheme = NewObject(); + RawControlScheme->Agent = ControlledAgent; + ControlSchemes.Add(RawControlScheme); + AddControlSchemes(); +} + +void AHolodeckPawnController::Tick(float DeltaSeconds) { + Super::Tick(DeltaSeconds); + + if (Server == nullptr) { + UE_LOG(LogHolodeck, Fatal, TEXT("HolodeckPawnController couldn't find server...")); + } + + if (ShouldChangeStateBuffer && *ShouldChangeStateBuffer & 0x4) { + ExecuteSetState(); + } + else if (ShouldChangeStateBuffer && *ShouldChangeStateBuffer) { + ExecuteTeleport(); + } + + unsigned int index = *ControlSchemeIdBuffer % ControlSchemes.Num(); + ControlSchemes[index]->Execute(ControlledAgent->GetRawActionBuffer(), ActionBuffer, DeltaSeconds); +} + +void AHolodeckPawnController::UpdateServerInfo() { + if (Server != nullptr) return; + UHolodeckGameInstance* Instance = static_cast(GetGameInstance()); + if (Instance != nullptr) + Server = Instance->GetServer(); + else + UE_LOG(LogHolodeck, Warning, TEXT("Game Instance is not UHolodeckGameInstance.")); +} + +void AHolodeckPawnController::AllocateBuffers(const FString& AgentName) { + UpdateServerInfo(); + if (Server != nullptr) { + if (!ControlledAgent) { + // LOG HERE + return; + } + + unsigned int MaxControlSize = 0; + for (UHolodeckControlScheme* const ControlScheme : ControlSchemes) { + if (ControlScheme->GetControlSchemeSizeInBytes() > MaxControlSize) + MaxControlSize = ControlScheme->GetControlSchemeSizeInBytes(); + } + + void* TempBuffer; + ActionBuffer = Server->Malloc(TCHAR_TO_UTF8(*AgentName), + MaxControlSize); + + TempBuffer = Server->Malloc(UHolodeckServer::MakeKey(AgentName, CONTROL_SCHEME_KEY), + sizeof(uint8)); + ControlSchemeIdBuffer = static_cast(TempBuffer); + + TempBuffer = Server->Malloc(UHolodeckServer::MakeKey(AgentName, TELEPORT_FLAG_KEY), + sizeof(uint8)); + ShouldChangeStateBuffer = static_cast(TempBuffer); + + TempBuffer = Server->Malloc(UHolodeckServer::MakeKey(AgentName, TELEPORT_COMMAND_KEY), + TELEPORT_COMMAND_SIZE * sizeof(float)); + TeleportBuffer = static_cast(TempBuffer); + } +} + +void AHolodeckPawnController::ExecuteTeleport() { + UE_LOG(LogHolodeck, Log, TEXT("Executing teleport")); + if (ControlledAgent == nullptr) { + UE_LOG(LogHolodeck, Warning, TEXT("AHolodeckPawnController::ExecuteTeleport: Couldn't get reference to controlled HolodeckAgent")); + return; + } + + float* FloatPtr = static_cast(TeleportBuffer); + + FVector TeleportLocation; + if (*ShouldChangeStateBuffer & 0x1) { + TeleportLocation = FVector(FloatPtr[0], FloatPtr[1], FloatPtr[2]); + TeleportLocation = ConvertLinearVector(TeleportLocation, ClientToUE); + } else { + TeleportLocation = ControlledAgent->GetActorLocation(); + } + + FRotator NewRotation; + if (*ShouldChangeStateBuffer & 0x2) { + NewRotation = RPYToRotator(FloatPtr[3], FloatPtr[4], FloatPtr[5]); + } else { + NewRotation = ControlledAgent->GetActorRotation(); + } + + ControlledAgent->Teleport(TeleportLocation, NewRotation); + *ShouldChangeStateBuffer = 0; +} + + +void AHolodeckPawnController::ExecuteSetState() { + UE_LOG(LogHolodeck, Log, TEXT("AHolodeckPawnController::ExecuteSetState")); + + if (ControlledAgent == nullptr) { + UE_LOG(LogHolodeck, Fatal, TEXT("AHolodeckPawnController::ExecuteSetState: Couldn't get reference to controlled HolodeckAgent")); + return; + } + + float* FloatPtr = static_cast(TeleportBuffer); + FVector TeleportLocation = FVector(FloatPtr[0], FloatPtr[1], FloatPtr[2]); + FRotator NewRotation = RPYToRotator(FloatPtr[3], FloatPtr[4], FloatPtr[5]); + FVector NewVelocity = FVector(FloatPtr[6], FloatPtr[7], FloatPtr[8]); + FVector NewAngVelocity = FVector(FloatPtr[9], FloatPtr[10], FloatPtr[11]); + + // Perform conversion + TeleportLocation = ConvertLinearVector(TeleportLocation, ClientToUE); + NewVelocity = ConvertLinearVector(NewVelocity, ClientToUE); + NewAngVelocity = ConvertAngularVector(NewAngVelocity, NoScale); + + ControlledAgent->SetState(TeleportLocation, NewRotation, NewVelocity, NewAngVelocity); + *ShouldChangeStateBuffer = 0; +} + +bool AHolodeckPawnController::CheckBoolBuffer(void* Buffer) { + bool* BoolPtr = static_cast(Buffer); + if (BoolPtr && *BoolPtr) { + *BoolPtr = false; + return true; + } + return false; +} + +void AHolodeckPawnController::SetServer(UHolodeckServer* const ServerParam) { + this->Server = ServerParam; +} + +UHolodeckServer* AHolodeckPawnController::GetServer() { + return this->Server; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSensor.cpp new file mode 100644 index 0000000000..54329671d6 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSensor.cpp @@ -0,0 +1,38 @@ +// Written by joshgreaves. + +#include "Holodeck.h" +#include "HolodeckSensor.h" + +UHolodeckSensor::UHolodeckSensor() { + PrimaryComponentTick.bCanEverTick = true; + //Sensors should tick after physics is processed, so that the data + //they collect is current. + PrimaryComponentTick.TickGroup = TG_PostPhysics; + bOn = true; +} + +void UHolodeckSensor::SetAgentAndController(AHolodeckPawnControllerInterface* ControllerParam, FString AgentNameParam) { + Controller = ControllerParam; + AgentName = AgentNameParam; +} + +void UHolodeckSensor::InitializeSensor() { + + if (bOn && Controller != nullptr) { + UE_LOG(LogTemp, Warning, TEXT("Getting buffer of size %d"), GetNumItems() * GetItemSize()); + Buffer = Controller->GetServer()->Malloc(UHolodeckServer::MakeKey(AgentName, SensorName + SensorDataKey), GetNumItems() * GetItemSize()); + } else { + UE_LOG(LogTemp, Warning, TEXT("Getting Controller Failed. Sensor not ")); + } +} + +void UHolodeckSensor::BeginPlay() { + Super::BeginPlay(); +} + +void UHolodeckSensor::TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + Super::TickComponent(DeltaTime, TickType, ThisTickFunction); + + if (bOn && Buffer != nullptr) + TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckServer.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckServer.cpp new file mode 100644 index 0000000000..88ab1db95b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckServer.cpp @@ -0,0 +1,126 @@ +// Created by joshgreaves on 5/9/17. + +#include "Holodeck.h" +#include "HolodeckServer.h" + +UHolodeckServer::UHolodeckServer() { + // Warning -- This class gets initialized a few times by Unreal because it is a UObject. + // DO NOT rely on singleton-qualities in the constructor, only in the Start() function + bIsRunning = false; +} + +UHolodeckServer::~UHolodeckServer() { + Kill(); +} + +void UHolodeckServer::Start() { + UE_LOG(LogHolodeck, Log, TEXT("Initializing HolodeckServer")); + if (bIsRunning) { + UE_LOG(LogHolodeck, Warning, TEXT("HolodeckServer is already running! Bringing it down and up")); + Kill(); + } + + if (!FParse::Value(FCommandLine::Get(), TEXT("HolodeckUUID="), UUID)) + UUID = ""; + UE_LOG(LogHolodeck, Log, TEXT("UUID: %s"), *UUID); + +#if PLATFORM_WINDOWS + auto LoadingSemaphore = OpenSemaphore(EVENT_ALL_ACCESS, false, *(LOADING_SEMAPHORE_PATH + UUID)); + ReleaseSemaphore(LoadingSemaphore, 1, NULL); + this->LockingSemaphore1 = CreateSemaphore(NULL, 1, 1, *(SEMAPHORE_PATH1 + UUID)); + this->LockingSemaphore2 = CreateSemaphore(NULL, 0, 1, *(SEMAPHORE_PATH2 + UUID)); +#elif PLATFORM_LINUX + auto LoadingSemaphore = sem_open(TCHAR_TO_ANSI(*(LOADING_SEMAPHORE_PATH + UUID)), O_CREAT, 0777, 0); + if (LoadingSemaphore == SEM_FAILED) { + LogSystemError("Unable to open loading semaphore"); + } + + LockingSemaphore1 = sem_open(TCHAR_TO_ANSI(*(SEMAPHORE_PATH1 + UUID)), O_CREAT, 0777, 1); + if (LockingSemaphore1 == SEM_FAILED) { + LogSystemError("Unable to open server semaphore"); + } + + LockingSemaphore2 = sem_open(TCHAR_TO_ANSI(*(SEMAPHORE_PATH2 + UUID)), O_CREAT, 0777, 0); + if (LockingSemaphore2 == SEM_FAILED) { + LogSystemError("Unable to open client semaphore"); + } + + int status = sem_post(LoadingSemaphore); + if (status == -1) { + LogSystemError("Unable to update loading semaphore"); + } + + // Client unlinks LoadingSemaphore +#endif + + bIsRunning = true; + UE_LOG(LogHolodeck, Log, TEXT("HolodeckServer started successfully")); +} + +void UHolodeckServer::Kill() { + UE_LOG(LogHolodeck, Log, TEXT("Killing HolodeckServer")); + if (!bIsRunning) return; + + Memory.clear(); + +#if PLATFORM_WINDOWS + CloseHandle(this->LockingSemaphore1); + CloseHandle(this->LockingSemaphore2); +#elif PLATFORM_LINUX + int status = sem_unlink(SEMAPHORE_PATH1); + if (status == -1) { + LogSystemError("Unable to close server semaphore"); + } + + status = sem_unlink(SEMAPHORE_PATH2); + if (status == -1) { + LogSystemError("Unable to close client semaphore"); + } +#endif + + bIsRunning = false; + UE_LOG(LogHolodeck, Log, TEXT("HolodeckServer successfully shut down")); +} + +void* UHolodeckServer::Malloc(const std::string& Key, unsigned int BufferSize) { + // If this key doesn't already exist, or the buffer size has changed, allocate the memory. + if (!Memory.count(Key) || Memory[Key]->Size() != BufferSize) { + UE_LOG(LogHolodeck, Log, TEXT("Mallocing %u bytes for key %s"), BufferSize, UTF8_TO_TCHAR(Key.c_str())); + Memory[Key] = std::unique_ptr(new HolodeckSharedMemory(Key, BufferSize, TCHAR_TO_UTF8(*UUID))); + } + return Memory[Key]->GetPtr(); +} + +void UHolodeckServer::Acquire() { + UE_LOG(LogHolodeck, VeryVerbose, TEXT("HolodeckServer Acquiring")); +#if PLATFORM_WINDOWS + WaitForSingleObject(this->LockingSemaphore1, INFINITE); +#elif PLATFORM_LINUX + + int status = sem_wait(LockingSemaphore1); + if (status == -1) { + LogSystemError("Unable to wait for server semaphore"); + } +#endif +} + +void UHolodeckServer::Release() { + UE_LOG(LogHolodeck, VeryVerbose, TEXT("HolodeckServer Releasing")); +#if PLATFORM_WINDOWS + ReleaseSemaphore(this->LockingSemaphore2, 1, NULL); +#elif PLATFORM_LINUX + + int status = sem_post(LockingSemaphore2); + if (status == -1) { + LogSystemError("Unable to update loading semaphore"); + } +#endif +} + +bool UHolodeckServer::IsRunning() const { + return bIsRunning; +} + +void UHolodeckServer::LogSystemError(const std::string& errorMessage) { + UE_LOG(LogHolodeck, Fatal, TEXT("%s - Error code: %d=%s"), ANSI_TO_TCHAR(errorMessage.c_str()), errno, ANSI_TO_TCHAR(strerror(errno))); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSharedMemory.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSharedMemory.cpp new file mode 100644 index 0000000000..edc98173d5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSharedMemory.cpp @@ -0,0 +1,100 @@ +// +// Created by joshgreaves on 5/9/17. +// + +#include "Holodeck.h" +#include "HolodeckSharedMemory.h" + +const char HOLODECK_BASE_PATH[] = "/HOLODECK_MEM"; + +#if PLATFORM_WINDOWS +// Returns the last Win32 error, in string format. Returns an empty string if there is no error. +std::string GetLastErrorAsString() +{ + // Get the error message, if any. + DWORD errorMessageID = GetLastError(); + if (errorMessageID == 0) { + return "NO ERROR - NO COLLUSION"; // No error message has been recorded + } + + UE_LOG(LogHolodeck, Error, TEXT("Error: %d"), errorMessageID); + + LPSTR messageBuffer = nullptr; + size_t size = FormatMessageA(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, + NULL, errorMessageID, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPSTR)&messageBuffer, 0, NULL); + + std::string message(messageBuffer, size); + + // Free the buffer. + LocalFree(messageBuffer); + + return message; +} +#endif + +HolodeckSharedMemory::HolodeckSharedMemory(const std::string& Name, unsigned int BufferSize, const std::string& UUID) : + MemPath(HOLODECK_BASE_PATH + UUID + "_" + Name), MemSize(BufferSize) { + + #if PLATFORM_WINDOWS + + std::wstring STemp = std::wstring(MemPath.begin(), MemPath.end()); + LPCWSTR WindowsMemPath = STemp.c_str(); + + UE_LOG(LogHolodeck, Log, TEXT("HolodeckSharedMemory:: Creating file mapping of size %d with path %s..."), BufferSize, ANSI_TO_TCHAR(MemPath.c_str())); + + MemFile = CreateFileMapping(INVALID_HANDLE_VALUE, NULL, PAGE_READWRITE, 0, this->MemSize, WindowsMemPath); + + if (GetLastError() == ERROR_ALREADY_EXISTS) { + UE_LOG(LogHolodeck, Warning, TEXT("HolodeckSharedMemory:: Unable to create MemFile! The path %s already exists. Hopefully it is the correct size..."), ANSI_TO_TCHAR(MemPath.c_str())); + } + + if (!MemFile) { + std::string msg = GetLastErrorAsString(); + UE_LOG(LogHolodeck, Fatal, TEXT("HolodeckSharedMemory:: Unable to create MemFile! %s"), ANSI_TO_TCHAR(msg.c_str())); + } + + MemPointer = static_cast(MapViewOfFile(MemFile, FILE_MAP_ALL_ACCESS, 0, 0, 0)); + + if (!MemPointer) { + std::string msg = GetLastErrorAsString(); + UE_LOG(LogHolodeck, Fatal, TEXT("HolodeckSharedMemory:: Unable to create MemPointer! %s"), ANSI_TO_TCHAR(msg.c_str())); + } + + #elif PLATFORM_LINUX + + MemFile = shm_open(MemPath.c_str(), O_CREAT | O_RDWR, 0777); + if (MemFile == -1) { + LogSystemError("Unable to create shared memory buffer"); + } + + int status = ftruncate(MemFile, this->MemSize); + if (status == -1) { + LogSystemError("Failed to truncate file"); + } + + MemPointer = static_cast(mmap(nullptr, this->MemSize, PROT_READ | PROT_WRITE, + MAP_SHARED, MemFile, 0)); + if (MemPointer == MAP_FAILED) { + LogSystemError("Failed to map shared memory"); + } + + // Doesn't need to stay open + close(MemFile); + #endif +} + +HolodeckSharedMemory::~HolodeckSharedMemory() { + #if PLATFORM_WINDOWS + CloseHandle(MemFile); + UnmapViewOfFile(MemPointer); + #elif PLATFORM_LINUX + // the client still hangs on to this memory location. We need to figure out a + // better way to release it. + // munmap(MemPointer, MemSize); + #endif +} + +void HolodeckSharedMemory::LogSystemError(const std::string &errorMessage) { + std::string errorMsg = errorMessage + " - Error code: " + std::to_string(errno) + " - " + std::string(strerror(errno)); + UE_LOG(LogHolodeck, Fatal, TEXT("%s"), ANSI_TO_TCHAR(errorMessage.c_str())); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSonar.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSonar.cpp new file mode 100644 index 0000000000..af597b7539 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckSonar.cpp @@ -0,0 +1,372 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "Benchmarker.h" +#include "HolodeckBuoyantAgent.h" +#include "HolodeckSonar.h" + +float UHolodeckSonar::ATan2Approx(float y, float x){ + //http://pubs.opengroup.org/onlinepubs/009695399/functions/atan2.html + //Volkan SALMA + + const float ONEQTR_PI = Pi / 4.0; + const float THRQTR_PI = 3.0 * Pi / 4.0; + float r, angle; + float abs_y = fabs(y) + 1e-10f; // kludge to prevent 0/0 condition + if ( x < 0.0f ) + { + r = (x + abs_y) / (abs_y - x); + angle = THRQTR_PI; + } + else + { + r = (x - abs_y) / (x + abs_y); + angle = ONEQTR_PI; + } + angle += (0.1963f * r * r - 0.9817f) * r; + angle *= 180/Pi; + if ( y < 0.0f ) + return( -angle ); // negate if in quad III or IV + else + return( angle ); +} + + +// Allows sensor parameters to be set programmatically from client. +void UHolodeckSonar::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + // Geometry Parameters + if (JsonParsed->HasTypedField("RangeMax")) { + RangeMax = JsonParsed->GetNumberField("RangeMax")*100; + } + if (JsonParsed->HasTypedField("RangeMin")) { + RangeMin = JsonParsed->GetNumberField("RangeMin")*100; + } + if (JsonParsed->HasTypedField("Azimuth")) { + Azimuth = JsonParsed->GetNumberField("Azimuth"); + } + if (JsonParsed->HasTypedField("Elevation")) { + Elevation = JsonParsed->GetNumberField("Elevation"); + } + + // Misc Parameters + if (JsonParsed->HasTypedField("InitOctreeRange")) { + InitOctreeRange = JsonParsed->GetNumberField("InitOctreeRange")*100; + } + if (JsonParsed->HasTypedField("TicksPerCapture")) { + TicksPerCapture = JsonParsed->GetIntegerField("TicksPerCapture"); + } + + // Visualization Parameters + if (JsonParsed->HasTypedField("ViewRegion")) { + ViewRegion = JsonParsed->GetBoolField("ViewRegion"); + } + if (JsonParsed->HasTypedField("ViewOctree")) { + ViewOctree = JsonParsed->GetIntegerField("ViewOctree"); + } + if (JsonParsed->HasTypedField("ShowWarning")) { + ShowWarning = JsonParsed->GetBoolField("ShowWarning"); + } + + // Performance Parameters + if (JsonParsed->HasTypedField("ShadowEpsilon")) { + ShadowEpsilon = JsonParsed->GetNumberField("ShadowEpsilon")*100; + } + if (JsonParsed->HasTypedField("WaterDensity")) { + WaterDensity = JsonParsed->GetNumberField("WaterDensity"); + } + if (JsonParsed->HasTypedField("WaterSpeedSound")) { + WaterSpeedSound = JsonParsed->GetNumberField("WaterSpeedSound"); + } + + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UHolodeckSonar::ParseSensorParms:: Unable to parse json.")); + } + + if(InitOctreeRange == 0){ + InitOctreeRange = RangeMax; + } + + if(ShadowEpsilon == 0){ + ShadowEpsilon = 4*Octree::OctreeMin; + } + + minAzimuth = -Azimuth/2; + maxAzimuth = Azimuth/2; + minElev = 90 - Elevation/2; + maxElev = 90 + Elevation/2; + + WaterImpedance = WaterDensity * WaterSpeedSound; + + sqrt3_2 = UKismetMathLibrary::Sqrt(3) / 2; + sinOffset = UKismetMathLibrary::DegSin(FGenericPlatformMath::Min(Azimuth, Elevation)/2); +} + +void UHolodeckSonar::BeginDestroy() { + Super::BeginDestroy(); + + delete octree; +} + +void UHolodeckSonar::initOctree(){ + // We delay making trees till the message has been printed to the screen + if(toMake.Num() != 0 && TickCounter >= 8){ + UE_LOG(LogHolodeck, Log, TEXT("Sonar::Initial building num: %d"), toMake.Num()); + ParallelFor(toMake.Num(), [&](int32 i){ + toMake.GetData()[i]->load(); + toMake.GetData()[i]->unload(); + }); + toMake.Empty(); + GEngine->AddOnScreenDebugMessage(-1, 2.0f, FColor::Red, TEXT("Finished.")); + } + + // If we haven't made it yet + if(octree == nullptr && TickCounter >= 5){ + // initialize small octree for each agent + for(auto& agent : Controller->GetServer()->AgentMap){ + // skip ourselves + if(agent.Value == this->GetAttachmentRootActor()) continue; + AHolodeckBuoyantAgent* bouyantActor = static_cast(agent.Value); + Octree* l = bouyantActor->makeOctree(); + if(l) agents.Add(l); + } + + // Ignore necessary agents to make world one + for(auto& agent : Controller->GetServer()->AgentMap){ + AActor* actor = static_cast(agent.Value); + Octree::ignoreActor(actor); + } + // make/load octree + octree = Octree::makeEnvOctreeRoot(); + + // Premake octrees within range + FVector loc = this->GetComponentLocation(); + // Offset by size of OctreeMax radius to get everything in range + float offset = InitOctreeRange + Octree::OctreeMax*FMath::Sqrt(3)/2; + // recursively search for close leaves + std::function&)> findCloseLeaves; + findCloseLeaves = [&offset, &loc, &findCloseLeaves](Octree* tree, TArray& list){ + if(tree->size == Octree::OctreeMax){ + if((loc - tree->loc).Size() <= offset && !FPaths::FileExists(tree->file)){ + list.Add(tree); + } + } + else{ + for(Octree* l : tree->leaves){ + findCloseLeaves(l, list); + } + } + }; + findCloseLeaves(octree, toMake); + if(toMake.Num() != 0) + GEngine->AddOnScreenDebugMessage(-1, 2.0f, FColor::Red, FString::Printf(TEXT("Premaking %d Octrees, will take some time..."), toMake.Num())); + + // get all our Leaves ready + bigLeaves.Reserve(1000); + // We need enough foundLeaves to cover all possible threads + // 1000 should be plenty + foundLeaves.Reserve(1000); + for(int i=0;i<1000;i++){ + foundLeaves.Add(TArray()); + foundLeaves[i].Reserve(10000); + } + } +} + +void UHolodeckSonar::viewLeaves(Octree* tree){ + if(tree->leaves.Num() == 0){ + DrawDebugPoint(GetWorld(), tree->loc, 5, FColor::Red, false, .03*TicksPerCapture); + } + else{ + for( Octree* l : tree->leaves){ + viewLeaves(l); + } + } +} + +bool UHolodeckSonar::inRange(Octree* tree){ + FTransform SensortoWorld = this->GetComponentTransform(); + // if it's not a leaf, we use a bigger search area + float offset = 0; + float radius = 0; + if(tree->size != Octree::OctreeMin){ + radius = tree->size*sqrt3_2; + offset = radius/sinOffset; + SensortoWorld.AddToTranslation( -this->GetForwardVector()*offset ); + } + + // transform location to sensor frame + // FVector locLocal = SensortoWorld.InverseTransformPositionNoScale(tree->loc); + FVector locLocal = SensortoWorld.GetRotation().UnrotateVector(tree->loc-SensortoWorld.GetTranslation()); + + // check if it's in range + tree->locSpherical.X = locLocal.Size(); + if(RangeMin+offset-radius >= tree->locSpherical.X || tree->locSpherical.X >= RangeMax+offset+radius) return false; + + // check if azimuth is in + tree->locSpherical.Y = ATan2Approx(-locLocal.Y, locLocal.X); + if(minAzimuth >= tree->locSpherical.Y || tree->locSpherical.Y >= maxAzimuth) return false; + + // check if elevation is in + tree->locSpherical.Z = ATan2Approx(locLocal.Size2D(), locLocal.Z); + if(minElev >= tree->locSpherical.Z || tree->locSpherical.Z >= maxElev) return false; + + // otherwise it's in! + return true; +} + +void UHolodeckSonar::leavesInRange(Octree* tree, TArray& rLeaves, float stopAt){ + bool in = inRange(tree); + if(in){ + if(tree->size == stopAt){ + if(stopAt == Octree::OctreeMin){ + // Compute contribution while we're parallelized + // If no contribution, we don't have to add it in + tree->normalImpact = GetComponentLocation() - tree->loc; + tree->normalImpact.Normalize(); + + // compute contribution + float cos = FVector::DotProduct(tree->normal, tree->normalImpact); + if(cos > 0){ + tree->cos = cos; + rLeaves.Add(tree); + } + } + else{ + rLeaves.Add(tree); + } + return; + } + + for(Octree* l : tree->leaves){ + leavesInRange(l, rLeaves, stopAt); + } + } + else if(tree->size >= Octree::OctreeMax){ + tree->unload(); + } +} + +void UHolodeckSonar::findLeaves(){ + // Empty everything out + bigLeaves.Reset(); + for(auto& fl: foundLeaves){ + fl.Reset(); + } + + // FILTER TO GET THE bigLeaves WE WANT + leavesInRange(octree, bigLeaves, Octree::OctreeMax); + bigLeaves += agents; + + ParallelFor(bigLeaves.Num(), [&](int32 i){ + Octree* leaf = bigLeaves.GetData()[i]; + leaf->load(); + for(Octree* l : leaf->leaves) + leavesInRange(l, foundLeaves.GetData()[i%1000], Octree::OctreeMin); + }); +} + +void UHolodeckSonar::shadowLeaves(){ + ParallelFor(sortedLeaves.Num(), [&](int32 i){ + TArray& binLeafs = sortedLeaves.GetData()[i]; + + // sort from closest to farthest + binLeafs.Sort([](const Octree& a, const Octree& b){ + return a.locSpherical.X < b.locSpherical.X; + }); + + // Get the closest cluster in the bin + float diff, R; + Octree* jth; + for(int32 j=0;jz - WaterImpedance) / (jth->z + WaterImpedance); + jth->val = R*R*jth->cos; + + // diff = FVector::Dist(jth->loc, binLeafs.GetData()[j+1]->loc); + if(j != binLeafs.Num()-1){ + diff = FMath::Abs(jth->locSpherical.X - binLeafs.GetData()[j+1]->locSpherical.X); + if(diff > ShadowEpsilon){ + binLeafs.RemoveAt(j+1,binLeafs.Num()-j-1); + break; + } + } + } + + }); +} + +void UHolodeckSonar::showBeam(float DeltaTime){ + // draw points inside our region + if(ViewOctree >= -1){ + for( TArray bins : sortedLeaves){ + for( Octree* l : bins){ + if(ViewOctree == -1 || ViewOctree == l->idx.Y){ + DrawDebugPoint(GetWorld(), l->loc, 5, FColor::Red, false, DeltaTime*TicksPerCapture); + } + } + } + } +} + +void UHolodeckSonar::showRegion(float DeltaTime){ + // draw outlines of our region + if(ViewRegion){ + FTransform tran = this->GetComponentTransform(); + float debugThickness = 3.0f; + + // range lines + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, minAzimuth, minElev, tran), spherToEuc(RangeMax, minAzimuth, minElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, minAzimuth, maxElev, tran), spherToEuc(RangeMax, minAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, maxAzimuth, minElev, tran), spherToEuc(RangeMax, maxAzimuth, minElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, maxAzimuth, maxElev, tran), spherToEuc(RangeMax, maxAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + + // azimuth lines (should be arcs, we're being lazy) + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, minAzimuth, minElev, tran), spherToEuc(RangeMin, maxAzimuth, minElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, minAzimuth, maxElev, tran), spherToEuc(RangeMin, maxAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMax, minAzimuth, minElev, tran), spherToEuc(RangeMax, maxAzimuth, minElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMax, minAzimuth, maxElev, tran), spherToEuc(RangeMax, maxAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + + // elevation lines (should be arcs, we're being lazy) + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, minAzimuth, minElev, tran), spherToEuc(RangeMin, minAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMin, maxAzimuth, minElev, tran), spherToEuc(RangeMin, maxAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMax, minAzimuth, minElev, tran), spherToEuc(RangeMax, minAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + DrawDebugLine(GetWorld(), spherToEuc(RangeMax, maxAzimuth, minElev, tran), spherToEuc(RangeMax, maxAzimuth, maxElev, tran), FColor::Green, false, DeltaTime*TicksPerCapture, ECC_WorldStatic, debugThickness); + } +} + +FVector UHolodeckSonar::spherToEuc(float r, float theta, float phi, FTransform SensortoWorld){ + float x = r*UKismetMathLibrary::DegSin(phi)*UKismetMathLibrary::DegCos(theta); + float y = r*UKismetMathLibrary::DegSin(phi)*UKismetMathLibrary::DegSin(theta); + float z = r*UKismetMathLibrary::DegCos(phi); + return UKismetMathLibrary::TransformLocation(SensortoWorld, FVector(x, y, z)); +} + +void UHolodeckSonar::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + // We initialize this here to make sure all agents are loaded + // This does nothing if it's already been loaded + initOctree(); + + // If the sonar ran last timestep, show off what it saw + if(TickCounter == 0){ + showBeam(DeltaTime); + showRegion(DeltaTime); + } + + // Count till next sonar timestep + TickCounter++; + // Show warning when we're about to compute sonar + if(ShowWarning && TicksPerCapture - TickCounter <= 3){ + GEngine->AddOnScreenDebugMessage(-1, DeltaTime, FColor::Red, FString::Printf(TEXT("Sonar octree generation may slow down simulation, screen may appear frozen temporarily"))); + } + if(TickCounter % TicksPerCapture == 0 && octree != nullptr && toMake.Num() == 0){ + TickCounter = 0; + } +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckWorldSettings.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckWorldSettings.cpp new file mode 100644 index 0000000000..57f752e735 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Private/HolodeckWorldSettings.cpp @@ -0,0 +1,16 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckWorldSettings.h" + +float AHolodeckWorldSettings::FixupDeltaSeconds(float DeltaSeconds, float RealDeltaSeconds) { + return ConstantTimeDeltaBetweenTicks; +} + +float AHolodeckWorldSettings::GetConstantTimeDeltaBetweenTicks() { + return ConstantTimeDeltaBetweenTicks; +} + +void AHolodeckWorldSettings::SetConstantTimeDeltaBetweenTicks(float Delta) { + ConstantTimeDeltaBetweenTicks = Delta; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgent.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgent.h new file mode 100644 index 0000000000..d348e448c5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgent.h @@ -0,0 +1,132 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "GameFramework/Pawn.h" +#include "HolodeckAgentInterface.h" +#include "Conversion.h" +#include "HolodeckPawnController.h" +#include "HolodeckPawnControllerInterface.h" +#include "HolodeckGameInstance.h" +#include "HolodeckAgent.generated.h" + + +/* Forward declare Holodeck Sensor Class. */ +class UHolodeckSensor; + +/** +* AHolodeckAgent +* The base class for holodeck agents. +* HolodeckAgents are controllable from python. +* A HolodeckAgent contains the logic for how it acts at a low level. +* To get to a higher level of control, the logic should be implemented in +* a HolodeckControlScheme. +*/ +UCLASS() +class HOLODECK_API AHolodeckAgent : public AHolodeckAgentInterface { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHolodeckAgent(); + + /** + * BeginPlay + * Should only call Super() and InitializeAgent() all other initialization + * code should be called from within that function + */ + virtual void BeginPlay() final; + + /** + * InitializeAgent + * All agent initialization code should go in here. + */ + UFUNCTION(BlueprintCallable) + virtual void InitializeAgent(); + + /** + * Tick + * Ticks the agent. + * If it is overridden, it must be called by the child class! + * @param DeltaSeconds the time since the last tick. + */ + virtual void Tick(float DeltaSeconds) override; + + /** + * Teleport + * Instantly moves the agent to target location, with the orientation that was given + * If no orientation was given, orientation remains unchanged (see overloaded function) + * @param NewLocation The location to move to + * @param NewRotation The rotation that the object will take on + * @return Bool if the teleport was successful. + */ + bool Teleport(const FVector& NewLocation, const FRotator& NewRotation) override; + + /** + * Teleport + * Instantly moves the agent to target location, with the orientation that was given + * Orientation remains unchanged + * @param NewLocation The location to move to + * @return Bool if the teleport was successful. + */ + bool Teleport(const FVector& NewLocation) override; + + /** + * SetState + * Sets the state of the agent (pos,rot,vel,ang_vel) + * @param NewLocation The location to move to + * @param NewRotation The rotation that the object will take on + * @param NewVelocity The new linear velocity + * @param NewAngVelocity The new angular velocity + * @return Bool if the teleport was successful. + */ + bool SetState(const FVector& NewLocation, const FRotator& NewRotation, const FVector& NewVelocity, const FVector& NewAngVelocity) override; + + /** + * InitializeController + * Hooks up everything with the controller. This is normally called in the beginPlay function, + * but if you have to manually configure a controller, you will have to call this function after + * you do it. + */ + bool InitializeController() override; + + /** + * GetRawActionSizeInBytes + * @return the number of bytes used by the action space. + */ + virtual unsigned int GetRawActionSizeInBytes() const override { + check(0 && "You must override GetRawActionSizeInBytes"); + return 0; + }; + + /** + * GetRawActionBuffer + * @return a pointer to the start of the action buffer. + */ + virtual void* GetRawActionBuffer() const override { + check(0 && "You must override GetRawActionBuffer"); + return nullptr; + }; + + /** + * GetAccelerationLimit + * @return Float The maximum acceleration the agent can endure before being abused. -1 means no limit. + */ + virtual float GetAccelerationLimit() { return -1; } + + UPROPERTY(BlueprintReadWrite) + bool IsAbused; + + /* Stores pointers to all the sensors on the agent. */ + TMap SensorMap; + AHolodeckPawnControllerInterface* HolodeckController; + +private: + + UHolodeckGameInstance* Instance; + UHolodeckServer* Server; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgentInterface.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgentInterface.h new file mode 100644 index 0000000000..93ecd4ec9c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckAgentInterface.h @@ -0,0 +1,112 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "GameFramework/Pawn.h" + +#include "HolodeckAgentInterface.generated.h" + +/** +* AHolodeckAgentInterface +* The base class for holodeck agents. +* HolodeckAgents are controllable from python. +* A HolodeckAgent contains the logic for how it acts at a low level. +* To get to a higher level of control, the logic should be implemented in +* a HolodeckControlScheme. +*/ +UCLASS() +class HOLODECK_API AHolodeckAgentInterface : public APawn { + GENERATED_BODY() + +public: + /** + * BeginPlay + * Called when the game starts. + */ + virtual void BeginPlay() override { Super::BeginPlay(); }; + + /** + * Tick + * Ticks the agent. + * If it is overridden, it must be called by the child class! + * @param DeltaSeconds the time since the last tick. + */ + virtual void Tick(float DeltaSeconds) override { Super::Tick(DeltaSeconds); }; + + /** + * SetState + * Sets the state of the agent (pos, rot, vel, ang_vel) + * @param NewLocation The location to move to + * @param NewRotation The rotation that the object will take on + * @param NewVelocity The new linear velocity + * @param NewAngVelocity The new angular velocity + * @return Bool if the teleport was successful. + */ + virtual bool SetState(const FVector& NewLocation, const FRotator& NewRotation, const FVector& NewVelocity, const FVector& NewAngVelocity){ + check(0 && "You must override SetState"); + return false; + }; + + /** + * Teleport + * Instantly moves the agent to target location, with the orientation that was given + * If no orientation was given, orientation remains unchanged (see overloaded function) + * @param NewLocation The location to move to + * @param NewRotation The rotation that the object will take on + * @return Bool if the teleport was successful. + */ + virtual bool Teleport(const FVector& NewLocation, const FRotator& NewRotation) { + check(0 && "You must override Teleport"); + return false; + }; + + /** + * Teleport + * Instantly moves the agent to target location, with the orientation that was given + * Orientation remains unchanged + * @param NewLocation The location to move to + * @return Bool if the teleport was successful. + */ + virtual bool Teleport(const FVector& NewLocation) { + check(0 && "You must override Teleport"); + return false; + }; + + /** + * InitializeController + * Hooks up everything with the controller. This is normally called in the beginPlay function, + * but if you have to manually configure a controller, you will have to call this function after + * you do it. + */ + virtual bool InitializeController() { + check(0 && "You must override InitializeController"); + return false; + }; + + /** + * GetRawActionSizeInBytes + * @return the number of bytes used by the action space. + */ + virtual unsigned int GetRawActionSizeInBytes() const { + check(0 && "You must override GetRawActionSizeInBytes"); + return 0; + }; + + /** + * GetRawActionBuffer + * @return a pointer to the start of the action buffer. + */ + virtual void* GetRawActionBuffer() const { + check(0 && "You must override GetRawActionBuffer"); + return nullptr; + }; + + // Must be set in the editor. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString AgentName; + + UPROPERTY(BlueprintReadWrite) + bool MainAgent; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckBuoyantAgent.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckBuoyantAgent.h new file mode 100644 index 0000000000..5b5cdc5bdd --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckBuoyantAgent.h @@ -0,0 +1,66 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "Containers/Array.h" +#include "Kismet/KismetMathLibrary.h" +#include "GameFramework/Pawn.h" +#include "HolodeckAgent.h" +#include "Octree.h" +#include "HolodeckBuoyantAgent.generated.h" + +/** + * + */ +UCLASS() +class HOLODECK_API AHolodeckBuoyantAgent : public AHolodeckAgent{ + GENERATED_BODY() + +public: + + virtual void BeginDestroy() override; + virtual void InitializeAgent() override; + + virtual void Tick(float DeltaSeconds) override; + + const float WaterDensity = 997; + float Gravity; + + UPROPERTY(BlueprintReadWrite, Category = UAVMesh) + UStaticMeshComponent* RootMesh; + + // Setting up coordinate position + // This probably should be set + FVector OffsetToOrigin = FVector(0,0,0); + + // Physical parameters of vehicle + // These all MUST be set + float Volume; // in m^3 + FVector CenterBuoyancy; // in cm + FVector CenterMass; // in cm + float MassInKG; + + // Used for surface buoyancy. + // These are optional to set, will be calculated based on mesh + FVector CenterVehicle = FVector(0,0,0); // Center of vehicle from origin. NEED to set if origin isn't center of vehicle + int NumSurfacePoints = 1000; + FBox BoundingBox = FBox(); + TArray SurfacePoints; + float SurfaceLevel = 0; + + void ApplyBuoyantForce(); + void ShowBoundingBox(); + void ShowSurfacePoints(); + + Octree* makeOctree(); + // octree in global coordinates in octree + Octree* octreeGlobal = nullptr; + // we store the octree in the actor coordinates in octreeClean, + Octree* octreeLocal = nullptr; + +private: + // Used to fix octreeGlobal + void updateOctree(Octree* localFrame, Octree* globalFrame); + // Used to extract local frame from global frame + Octree* cleanOctree(Octree* globalFrame); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckControlScheme.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckControlScheme.h new file mode 100644 index 0000000000..2156749272 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckControlScheme.h @@ -0,0 +1,21 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckControlScheme.generated.h" + +/** + * UHolodeckControlScheme + */ +UCLASS() +class HOLODECK_API UHolodeckControlScheme : public UObject { + GENERATED_BODY() + +public: + UHolodeckControlScheme(); + UHolodeckControlScheme(const FObjectInitializer& ObjectInitializer); + + virtual void Execute(void* const CommandArray, void* const InputCommand, float DeltaSeconds); + + virtual unsigned int GetControlSchemeSizeInBytes() const; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameInstance.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameInstance.h new file mode 100644 index 0000000000..326edcdc8a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameInstance.h @@ -0,0 +1,75 @@ +// Written by joshgreaves. + +#pragma once + +#include "Holodeck.h" + +#include "Engine/GameInstance.h" +#include "HolodeckServer.h" +#include "HolodeckWorldSettings.h" + +#include "HolodeckGameInstance.generated.h" + +/** +* UHolodeckGameInstance +* An instance of a holodeck game. +* It is a placeholder for important information, and it is also used for starting the server. +* tell their controller to open up the channels needed for giving commands across the python binding. +*/ +UCLASS() +class UHolodeckGameInstance : public UGameInstance +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UHolodeckGameInstance(const FObjectInitializer& ObjectInitializer); + + /** + * GetServer + * StartServer must be called before this is called. + * @return a pointer to the server, or nullptr if Holodeck is off. + */ + UHolodeckServer* GetServer(); + + /** + * StartServer + * Should only be called by HolodeckGameMode. + * Starts the server. + */ + void StartServer(); + + /** + * Tick + * Ticks the game instance. + * Is called by HolodeckGameMode. + * @param DeltaTime the seconds passed since the last tick. + */ + void Tick(float DeltaTime); + + /** + * Init + * Starts the GameInstance. + */ + void Init(); + + UPROPERTY(BlueprintReadWrite) + bool SpectatorMode; + UPROPERTY(BlueprintReadWrite) + bool DirectAttachMode; + UPROPERTY(BlueprintReadWrite) + int AttatchedAgentIndex = -1; + + UPROPERTY(BlueprintReadWrite) + FTransform SpectatorTransform; + + UPROPERTY(BlueprintReadWrite) + FVector ViewRotation; + +private: + UPROPERTY() + UHolodeckServer* Server; + AHolodeckWorldSettings* WorldSettings; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameMode.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameMode.h new file mode 100644 index 0000000000..6ec333ab04 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckGameMode.h @@ -0,0 +1,97 @@ +// Written by joshgreaves. + +#pragma once + +#include "Holodeck.h" + +#include "Octree.h" + +#include "GameFramework/GameMode.h" +#include "HolodeckGameInstance.h" +#include "CommandCenter.h" +#include "HolodeckGameMode.generated.h" + +/** + * AHolodeckGameMode + * The base game mode for Holodeck. + * HolodeckGameModeBP can be used to turn on and off Holodeck functionality + * from the editor. + */ +UCLASS() +class AHolodeckGameMode : public AGameMode +{ + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + explicit AHolodeckGameMode(const FObjectInitializer& ObjectInitializer); + + /** + * Tick + * Ticks the game mode. + * Checks the change in settings, and ticks the game instance. + * @param DeltaSeconds how many seconds passed since last tick. + */ + void Tick(float DeltaSeconds) override; + + /** + * StartPlay + * Called when the game begins. + * Registers all settings. + */ + void StartPlay() override; + + // Can be set off to turn off holodeck functionality. + UPROPERTY(EditAnywhere) + bool bHolodeckIsOn; + + /** + * GetAssociatedServer + * Returns the private server pointer that the instance contains. + */ + UHolodeckServer* GetAssociatedServer() { return this->Server; }; + + // These functions allow the Holodeck to do things which cannot normally be done from pure c++ code + UFUNCTION(BlueprintImplementableEvent) + AHolodeckAgent* SpawnAgent(const FString& Type, const FVector& Location, const FRotator& Rotation, const FString& Name, bool IsMainAgent); + + UFUNCTION(BlueprintImplementableEvent) + void TeleportCamera(const FVector& Location, const FVector& Rotation); + + UFUNCTION(BlueprintImplementableEvent) + void ExecuteCustomCommand(const FString& Name, const TArray& NumberParameters, const TArray& StringParameters); + + UFUNCTION(BlueprintImplementableEvent) + AActor* FindActorWithTag(const FString& Tag); + + UFUNCTION(BlueprintImplementableEvent) + float GetWorldNum(const FString& Key); + + UFUNCTION(BlueprintImplementableEvent) + FString GetWorldString(const FString& Key); + + UFUNCTION(BlueprintImplementableEvent) + bool GetWorldBool(const FString& Key); + + UFUNCTION(BlueprintCallable) + void LogFatalMessage(const FString& Message); + +private: + /** + * RegisterSettings + * Registers all the settings on the server. + */ + void RegisterSettings(); + + // Setting buffers + bool* ResetSignal; + + UPROPERTY() + UHolodeckServer* Server; + UPROPERTY() + UHolodeckGameInstance* Instance; + UPROPERTY() + UCommandCenter* CommandCenter; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnController.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnController.h new file mode 100644 index 0000000000..ad695fb355 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnController.h @@ -0,0 +1,126 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "AIController.h" +#include "HolodeckPawnControllerInterface.h" +#include "HolodeckAgentInterface.h" +#include "HolodeckControlScheme.h" +#include "RawControlScheme.h" +#include "HolodeckGameInstance.h" +#include "HolodeckServer.h" + +#include "HolodeckPawnController.generated.h" + +/** + * AHolodeckPawnController + * A controller for Holodeck Agents. + * If a HolodeckAgent doesn't contain this controller or a controller which + * inherits it, then you will run into problems. This class handles the + * subscribing of sensors and setting up action buffer channels. The + * sensors themselves tell the controller to subscribe them. Its base + * classes must report what size of action buffer they need. + * HolodeckPawnControllers get the data from the shared memory and give + * the commands to the pawns/agents. + */ +UCLASS() +class HOLODECK_API AHolodeckPawnController : public AHolodeckPawnControllerInterface +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHolodeckPawnController(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()); + + /** + * Default Destructor + */ + ~AHolodeckPawnController(); + + /** + * BeginPlay + * Called when the game starts. + * Registers the reward and terminal signals. + */ + void BeginPlay() override; + + /** + * Tick + * Ticks the agent. + * If it is overridden, it must be called by the child class! + * @param DeltaSeconds the time since the last tick. + */ + void Tick(float DeltaSeconds) override; + + /** + * OnPossess + * Called to Possess a pawn. + */ + void OnPossess(APawn* InPawn) override; + + /** + * GetActionBuffer + * Gets the action buffer for this agent. + * @param AgentName the name of the agent to subscribe an action buffer for. + + */ + void AllocateBuffers(const FString& AgentName); + + virtual void AddControlSchemes() override { check(0 && "You must override AddControlSchemes"); }; + + /** + * ExecuteTeleport + * Tells the controlled agent to teleport to the location in the shared memory. + */ + virtual void ExecuteTeleport() override; + + /** + * ExecuteSetState + * Sets a new state for the controlled agent + */ + virtual void ExecuteSetState() override; + + /** + * SetServer + * Sets the server object within this object. + */ + virtual void SetServer(UHolodeckServer* const ServerParam) override; + + /** + * GetServer + * Sets the server object within this object. + */ + virtual UHolodeckServer* GetServer() override; + +protected: + void* ActionBuffer; + uint8* ControlSchemeIdBuffer; + float* TeleportBuffer; + uint8* ShouldChangeStateBuffer; + + UPROPERTY() + TArray ControlSchemes; + AHolodeckAgentInterface* ControlledAgent; + +private: + /** + * GetServer + * Sets the server object within this object. + */ + void UpdateServerInfo(); + + /** + * CheckBoolBuffer + * Checks to see if the buffer is true or not, sets the buffer to false, + * then returns the value. + */ + bool CheckBoolBuffer(void* Buffer); + + const int SINGLE_BOOL = 1; + const int TELEPORT_COMMAND_SIZE = 12; + UHolodeckServer* Server; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnControllerInterface.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnControllerInterface.h new file mode 100644 index 0000000000..fff87354af --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckPawnControllerInterface.h @@ -0,0 +1,121 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "AIController.h" +#include "HolodeckServer.h" + +#include "HolodeckPawnControllerInterface.generated.h" + +/** + * AHolodeckPawnControllerInterface + * A controller for Holodeck Agents. + * If a HolodeckAgent doesn't contain this controller or a controller which + * inherits it, then you will run into problems. This class handles the + * subscribing of sensors and setting up action buffer channels. The + * sensors themselves tell the controller to subscribe them. Its base + * classes must report what size of action buffer they need. + * HolodeckPawnControllers get the data from the shared memory and give + * the commands to the pawns/agents. + */ +UCLASS() +class HOLODECK_API AHolodeckPawnControllerInterface : public AAIController +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + AHolodeckPawnControllerInterface(const FObjectInitializer& ObjectInitializer = FObjectInitializer::Get()) : + AAIController(ObjectInitializer) {}; + + /** + * Default Destructor + */ + ~AHolodeckPawnControllerInterface() {}; + + /** + * BeginPlay + * Called when the game starts. + * Registers the reward and terminal signals. + */ + virtual void BeginPlay() override { + Super::BeginPlay(); + }; + + /** + * Tick + * Ticks the agent. + * If it is overridden, it must be called by the child class! + * @param DeltaSeconds the time since the last tick. + */ + virtual void Tick(float DeltaSeconds) override { + Super::Tick(DeltaSeconds); + }; + + /** + * OnPossess + * Called to Possess a pawn. + */ + virtual void OnPossess(APawn* InPawn) { + Super::Possess(InPawn); + }; + + /** + * OnUnPossess + * Called to Unpossess a pawn + */ + virtual void OnUnPossess() { + Super::UnPossess(); + }; + + /** + * GetActionBuffer + * Gets the action buffer for this agent. + * @param AgentName the name of the agent to subscribe an action buffer for. + + */ + virtual void AllocateBuffers(const FString& AgentName) { + check(0 && "You must override AllocateBuffers"); + }; + + virtual void AddControlSchemes() { + check(0 && "You must override AddControlSchemes"); + }; + + /** + * ExecuteTeleport + * Tells the controlled agent to teleport to the location in the shared memory. + */ + virtual void ExecuteTeleport() { + check(0 && "You must override ExecuteTeleport"); + }; + + /** + * ExecuteSetState + * Sets a new state for the controller agent. + */ + virtual void ExecuteSetState() { + check(0 && "You must override ExecuteSetState"); + }; + + /** + * SetServer + * Sets the server object within this object. + */ + virtual void SetServer(UHolodeckServer* const ServerParam) { + check(0 && "You must override SetServer"); + }; + + /** + * GetServer + * gets the server object within this object. + */ + virtual UHolodeckServer* GetServer() { + check(0 && "You must override SetServer"); + return nullptr; + }; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h new file mode 100644 index 0000000000..6d16b0ede0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSensor.h @@ -0,0 +1,113 @@ +// Written by joshgreaves. + +#pragma once + +#include "HolodeckAgent.h" +#include "HolodeckPawnControllerInterface.h" +#include "Components/SceneComponent.h" +#include "HolodeckSensor.generated.h" + +/** + * HolodeckSensor + * Abstract base class for sensors within holodeck + * Handles publishing sensor data + * To function properly, HolodeckSensors must be attached to an agent that is controlled by + * a holodeck controller. + * To override, you must implement: + * GetDataKey + * GetDataLength + * TickSensorComponent + * And in BeginPlay you must call + * Super::BeginPlay + * But you cannot override: + * TickComponent + */ +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent), abstract ) +class HOLODECK_API UHolodeckSensor : public USceneComponent +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UHolodeckSensor(); + + /** + * Contains all sensor initialization code and subscribes the sensor + * Subclasses should make sure to call Super::InitializeSensor() + */ + virtual void InitializeSensor(); + + /** + * SetAgentAndController + * Sets the agent and controller + * Should be called before InitializeSensor() + */ + virtual void SetAgentAndController(AHolodeckPawnControllerInterface* ControllerParam, FString AgentName); + + /** + * BeginPlay + * Should only call Super::BeginPlay() and InitializeSensor() all other initialization + * code should be called from within that function + */ + virtual void BeginPlay() final; + + /** + * Publishes sensor data each tick + * Subclasses should NOT override this function. + * Instead, they should override TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) + * TickSensorComponent is called from this + */ + virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + /** + * Override this function if sensor has parameters to initialize. + */ + virtual void ParseSensorParms(FString ParmsJson) {} + + virtual FString GetAgentName() { return this->AgentName; } + + FString AgentName; + + // Allows you to modify the sensor name in the editor to allow for duplicate sensors on an agent + // Default SensorName should be set in child before calling Super::InitializeSensor + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString SensorName = ""; + + // Allows you to turn the sensor on and off in the editor + UPROPERTY(EditAnywhere) + bool bOn; + +protected: + + /** + * Must be overridden by subclass + * Set the number of data items + * e.g. return 100 + */ + virtual int GetNumItems() { check(0 && "You must override getNumItems"); return 0; }; + + /** + * Must be overridden by subclass + * Set the size of each data item + * e.g. return sizeof(float) + */ + virtual int GetItemSize() { check(0 && "You must override getItemSize"); return 0; }; + + /** + * TickSensorComponent + * Mimics Unreal Engine's UActorComponent::TickComponent function. See it's documentation for what the parameters are. + * Must be overridden by subclass + * Handles the logic for ticking the sensor + * Make sure to set the data, + * Cast Buffer into the right kind of pointer and set the data. + */ + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + check(0 && "You must override TickSensorComponent"); }; + + AHolodeckPawnControllerInterface* Controller; + void* Buffer; + + const FString SensorDataKey = "_sensor_data"; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckServer.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckServer.h new file mode 100644 index 0000000000..f1d400e7c5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckServer.h @@ -0,0 +1,139 @@ +// Created by joshgreaves on 5/9/17. + +#pragma once + +#include "Holodeck.h" + +#include +#include +#include +#include + +#include "HolodeckSharedMemory.h" +#if PLATFORM_WINDOWS +#define LOADING_SEMAPHORE_PATH "Global\\HOLODECK_LOADING_SEM" +#define SEMAPHORE_PATH1 "Global\\HOLODECK_SEMAPHORE_SERVER" +#define SEMAPHORE_PATH2 "Global\\HOLODECK_SEMAPHORE_CLIENT" +#include "AllowWindowsPlatformTypes.h" +#include "Windows.h" +#include "HideWindowsPlatformTypes.h" +#elif PLATFORM_LINUX +#define LOADING_SEMAPHORE_PATH "/HOLODECK_LOADING_SEM" +#define SEMAPHORE_PATH1 "/HOLODECK_SEMAPHORE_SERVER" +#define SEMAPHORE_PATH2 "/HOLODECK_SEMAPHORE_CLIENT" +#include +#include +#include +#include + +#endif + +#include "HolodeckServer.generated.h" + + +/* Forward declare HolodeckAgent Class*/ +class AHolodeckAgent; + +/** + * UHolodeckServer + * This class resides in Holodeck, and handles the passing of messages through + * shared memory. A new shared memory block is created for each sensor, action + * space, and setting. + * There should only be one UHolodeckServer, and it should be instantiated by + * HolodeckGameInstance. HolodeckGameMode calls HolodeckGameInstance::StartServer() + */ +UCLASS() +class HOLODECK_API UHolodeckServer : public UObject { + GENERATED_BODY() + +public: + + /** + * Default Constructor + */ + UHolodeckServer(); + + /** + * Default Destructor + */ + ~UHolodeckServer(); + + /** + * Start + * Starts the server. + */ + void Start(); + + /** + * Kill + * Shuts the server down. + */ + void Kill(); + + /** + * Malloc + * Mallocs shared memory. + * If memory has already been malloc'ed with the same key, + * the block is overwritten. + * @param Key the key for this block of memory. + * @param BufferSize the size to allocate in bytes. + * @return a pointer to the start of the assigned memory. + */ + void* Malloc(const std::string& Key, unsigned int BufferSize); + + /** + * Acquire + * Acquires the mutex to allow the next tick to occur. Will block until + * the client releases. + */ + void Acquire(); + + /** + * Release + * Releases the mutex to allow the client to tick. + */ + void Release(); + + /** + * IsRunning + * Gets whether the server is running. + * @return true if the server is running. + */ + bool IsRunning() const; + + /** + * MakeKey + * Makes a key for mallocing a specific item for a specific agent. + * @param AgentName the name of the agent. + * @param ItemName the name of the item. + * @return a key for this agent/item. + */ + static const std::string MakeKey(const std::string& AgentName, const std::string& ItemName) { + return AgentName + "_" + ItemName; + } + static const std::string MakeKey(const FString& AgentName, const std::string& ItemName) { + return std::string(TCHAR_TO_UTF8(*AgentName)) + "_" + ItemName; + } + static const std::string MakeKey(const FString& AgentName, const FString& ItemName) { + return std::string(TCHAR_TO_UTF8(*AgentName)) + "_" + std::string(TCHAR_TO_UTF8(*ItemName)); + } + + /* Stores pointers to all the agents within the world. */ + TMap AgentMap; + +private: + + FString UUID; + std::map> Memory; + bool bIsRunning; + + #if PLATFORM_WINDOWS + HANDLE LockingSemaphore1; + HANDLE LockingSemaphore2; + #elif PLATFORM_LINUX + sem_t* LockingSemaphore1; + sem_t* LockingSemaphore2; + #endif + + void LogSystemError(const std::string &errorMessage); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSharedMemory.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSharedMemory.h new file mode 100644 index 0000000000..e4d86aeac4 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSharedMemory.h @@ -0,0 +1,69 @@ +// +// Created by joshgreaves on 5/9/17. +// + +#pragma once + +#include +#include + +#if PLATFORM_WINDOWS +#include "AllowWindowsPlatformTypes.h" +#include +#include "HideWindowsPlatformTypes.h" +#elif PLATFORM_LINUX +#include +#include +#include +#include +#endif + +/** + * HolodeckSharedMemory + * A simple abstraction of memory mapped files for Windows + * and Linux. + */ +class HOLODECK_API HolodeckSharedMemory { +public: + /** + * Constructor + * Constructs a memory mapped file with a given name, prepended + * by "HOLODECK_MEM_". + * @param Name the name of the memory mapped file. Is prepended. + * @param BufferSize the number of bytes to allocated for the file. + * @param UUID a UUID for the Holodeck instance to allow multiple shared blocks on one machine. + */ + explicit HolodeckSharedMemory(const std::string& Name, unsigned int BufferSize, const std::string& UUID); + + /** + * Destructor + */ + ~HolodeckSharedMemory(); + + /** + * GetPtr + * Gets a pointer to the start of the memory mapped file. + * @return a void pointer to the start of the memory buffer. + */ + void* GetPtr() const {return MemPointer;} + + /** + * Size + * Gets the size of the allocated memory mapped file. + * @return the size in bytes of the file. + */ + int Size() const {return MemSize;} + +private: + std::string MemPath; + unsigned int MemSize; + void* MemPointer; + + #if PLATFORM_WINDOWS + HANDLE MemFile; + #elif PLATFORM_LINUX + int MemFile; + #endif + + void LogSystemError(const std::string &errorMessage); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSonar.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSonar.h new file mode 100644 index 0000000000..9220961fb8 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckSonar.h @@ -0,0 +1,135 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSensor.h" + +#include "GenericPlatform/GenericPlatformMath.h" +#include "Octree.h" +#include "Kismet/KismetMathLibrary.h" +#include "Async/ParallelFor.h" + +#include "HolodeckSonar.generated.h" + +#define Pi 3.1415926535897932384626433832795 +/** + * UHolodeckSonar + */ +UCLASS() +class HOLODECK_API UHolodeckSonar : public UHolodeckSensor +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UHolodeckSonar(){} + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + /* + * Cleans up octree + */ + virtual void BeginDestroy() override; + +protected: + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + float RangeMax = 1000; + + UPROPERTY(EditAnywhere) + float InitOctreeRange = 0; + + UPROPERTY(EditAnywhere) + float RangeMin = 10; + + UPROPERTY(EditAnywhere) + float Azimuth = 120; + + UPROPERTY(EditAnywhere) + float Elevation = 20; + + UPROPERTY(EditAnywhere) + int TicksPerCapture = 1; + + UPROPERTY(EditAnywhere) + bool ViewRegion = false; + + UPROPERTY(EditAnywhere) + int ViewOctree = -10; + + UPROPERTY(EditAnywhere) + float ShadowEpsilon = 0; + + UPROPERTY(EditAnywhere) + float WaterDensity = 997.0; + + UPROPERTY(EditAnywhere) + float WaterSpeedSound = 1480.0; + + UPROPERTY(EditAnywhere) + bool ShowWarning = true; + + // Call at the beginning of every tick, loads octree + void initOctree(); + + // Finds all the leaves in range + void findLeaves(); + + // Shadow leaves that have been sorted + void shadowLeaves(); + + // Visualizer helpers + void showBeam(float DeltaTime); + virtual void showRegion(float DeltaTime); + + // Used to hold leafs when parallelized filtering happens + TArray> foundLeaves; + + // Used to hold leafs when parallelized sorting/binning happens + TArray> sortedLeaves; + + // Water information + float WaterImpedance; + + // use for skipping frames + int TickCounter = 0; + + // various computations we want to cache + float ATan2Approx(float y, float x); + float minAzimuth; + float maxAzimuth; + float minElev; + float maxElev; + + virtual bool inRange(Octree* tree); + void leavesInRange(Octree* tree, TArray& leafs, float stopAt); + FVector spherToEuc(float r, float theta, float phi, FTransform SensortoWorld); + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; + + // holds our implementation of Octrees + Octree* octree = nullptr; + TArray agents; + void viewLeaves(Octree* tree); + + // What octrees we initally make + TArray toMake; + // initialize + reserve vectors once + TArray bigLeaves; + + // various computations we want to cache + float sqrt3_2; + float sinOffset; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckWorldSettings.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckWorldSettings.h new file mode 100644 index 0000000000..a3fa795540 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/HolodeckCore/Public/HolodeckWorldSettings.h @@ -0,0 +1,48 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "GameFramework/WorldSettings.h" +#include "HolodeckWorldSettings.generated.h" + +/** + * AHolodeckWorldSettings + * The global settings being used by the simulator. + * This is where the tick time is set. + */ +UCLASS() +class AHolodeckWorldSettings : public AWorldSettings +{ + GENERATED_BODY() + +public: + /** + * FixupDeltaSeconds + * Overidden to ensure a tick is 0.033 seconds. + */ + float FixupDeltaSeconds(float DeltaSeconds, float RealDeltaSeconds) override; + + AHolodeckWorldSettings() { + int TicksPerSec; + if (FParse::Value(FCommandLine::Get(), TEXT("TicksPerSec="), TicksPerSec)) { + ConstantTimeDeltaBetweenTicks = 1.0 / TicksPerSec; + } + }; + + /** + * GetConstantTimeDeltaBetweenTicks + * Gets the time between ticks. + * @return the time between ticks in seconds. + */ + float GetConstantTimeDeltaBetweenTicks(); + + /** + * SetConstantTimeDeltaBetweenTicks(float Delta) + * Sets the time of one tick. + * @param Delta the time for one tick. + */ + void SetConstantTimeDeltaBetweenTicks(float Delta); + +private: + float ConstantTimeDeltaBetweenTicks = 0.033; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AbuseSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AbuseSensor.cpp new file mode 100644 index 0000000000..f93a5adc72 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AbuseSensor.cpp @@ -0,0 +1,60 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "AbuseSensor.h" + +UAbuseSensor::UAbuseSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "AbuseSensor"; + + Agent = (AHolodeckAgent*)this->GetOwner(); +} + +void UAbuseSensor::InitializeSensor() { + Super::InitializeSensor(); +} + +void UAbuseSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + + this->AccelerationLimit = Agent->GetAccelerationLimit(); + + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("AccelerationLimit")) { + this->AccelerationLimit = JsonParsed->GetNumberField("AccelerationLimit"); + } + } + else { + UE_LOG(LogHolodeck, Warning, TEXT("%s Unable to parse json."), *FString(__func__)); + } +} + +void UAbuseSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (Agent != nullptr && bOn) { + int32 Abused = (int32)Agent->IsAbused; + + if (!Abused && AccelerationLimit != -1) { + FVector CurrentSpeed = Agent->GetVelocity(); + + // a = (delta v / delta t) + float Acceleration = (CurrentSpeed - PrevSpeed).Size() / DeltaTime; + Acceleration /= 100; // Convert cm/s2 to m/s2 + + if (Acceleration > AccelerationLimit) { + Abused = 1; + } + + PrevSpeed = CurrentSpeed; + } + + int* IntBuffer = static_cast(Buffer); + IntBuffer[0] = Abused; + + // IsAbused must be reset in case the agent leaves its abusive state + Agent->IsAbused = false; + } +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AcousticBeaconSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AcousticBeaconSensor.cpp new file mode 100644 index 0000000000..79a03b679f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/AcousticBeaconSensor.cpp @@ -0,0 +1,68 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "AcousticBeaconSensor.h" + +UAcousticBeaconSensor::UAcousticBeaconSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "AcousticBeaconSensor"; +} + +void UAcousticBeaconSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object the sensor is attached to. + Parent = Cast(this->GetAttachParent()); +} + +void UAcousticBeaconSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the velocity and buffer, then send the data to it. + if (Parent != nullptr && bOn) { + // if someone starting transmitting + if (fromSensor){ + // get coordinates of other sensor in local frame + FTransform SensortoWorld = this->GetComponentTransform(); + FVector fromLocation = fromSensor->GetComponentLocation(); + FVector fromLocationLocal = UKismetMathLibrary::InverseTransformLocation(SensortoWorld, fromLocation); + fromLocationLocal = ConvertLinearVector(fromLocationLocal, UEToClient); + + // calculate angles + WaitBuffer[0] = UKismetMathLibrary::Atan2(fromLocationLocal.Y, fromLocationLocal.X); + float dist = UKismetMathLibrary::Sqrt( FMath::Square(fromLocationLocal.X) + FMath::Square(fromLocationLocal.Y) ); + WaitBuffer[1] = UKismetMathLibrary::Atan2(fromLocationLocal.Z, dist); + + // distance away (already converted) + WaitBuffer[2] = fromLocationLocal.Size(); + + // Z Value + FVector toLocation = this->GetComponentLocation(); + WaitBuffer[3] = ConvertUnrealDistanceToClient( toLocation.Z ); + + // empty the sensor we got it from + fromSensor = NULL; + + // figure out how long to receive that message + WaitTicks = roundl(WaitBuffer[2] / (DeltaTime * SpeedOfSound)); + } + // if we're waiting for message + if(WaitTicks > 0){ + WaitTicks--; + } + // if the message was received + else if(WaitTicks == 0){ + float* FloatBuffer = static_cast(Buffer); + for(int i=0;i<4;i++) + FloatBuffer[i] = WaitBuffer[i]; + + WaitTicks = -1; + } + // remove message after it was received + else if(WaitTicks == -1){ + float* FloatBuffer = static_cast(Buffer); + for(int i=0;i<4;i++) + FloatBuffer[i] = std::numeric_limits::quiet_NaN(); + + WaitTicks = -2; + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DVLSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DVLSensor.cpp new file mode 100644 index 0000000000..80d36a4cb7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DVLSensor.cpp @@ -0,0 +1,151 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "DVLSensor.h" + +UDVLSensor::UDVLSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "DVLSensor"; +} + +void UDVLSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Elevation")) { + elevation = JsonParsed->GetNumberField("Elevation"); + } + if (JsonParsed->HasTypedField("ReturnRange")) { + ReturnRange = JsonParsed->GetBoolField("ReturnRange"); + } + if (JsonParsed->HasTypedField("MaxRange")) { + MaxRange = JsonParsed->GetNumberField("MaxRange")*100; + } + if (JsonParsed->HasTypedField("DebugLines")) { + DebugLines = JsonParsed->GetBoolField("DebugLines"); + } + + // For handling noise + if (JsonParsed->HasTypedField("VelSigma")) { + mvnVel.initSigma(JsonParsed->GetNumberField("VelSigma")); + } + if (JsonParsed->HasTypedField("VelSigma")) { + mvnVel.initSigma(JsonParsed->GetArrayField("VelSigma")); + } + if (JsonParsed->HasTypedField("VelCov")) { + mvnVel.initCov(JsonParsed->GetNumberField("VelCov")); + } + if (JsonParsed->HasTypedField("VelCov")) { + mvnVel.initCov(JsonParsed->GetArrayField("VelCov")); + } + + if (JsonParsed->HasTypedField("RangeSigma")) { + mvnRange.initSigma(JsonParsed->GetNumberField("RangeSigma")); + } + if (JsonParsed->HasTypedField("RangeSigma")) { + mvnRange.initSigma(JsonParsed->GetArrayField("RangeSigma")); + } + if (JsonParsed->HasTypedField("RangeCov")) { + mvnRange.initCov(JsonParsed->GetNumberField("RangeCov")); + } + if (JsonParsed->HasTypedField("RangeCov")) { + mvnRange.initCov(JsonParsed->GetArrayField("RangeCov")); + } + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UDVLSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void UDVLSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object the sensor is attached to. + Parent = Cast(this->GetAttachParent()); + + sinElev = UKismetMathLibrary::DegSin(elevation); + cosElev = UKismetMathLibrary::DegCos(elevation); + + // make transformation matrix + // See https://etda.libraries.psu.edu/files/final_submissions/17327 + transform = { {1/(2*sinElev), 0, -1/(2*sinElev), 0}, + { 0, 1/(2*sinElev), 0, -1/(2*sinElev)}, + {1/(4*cosElev), 1/(4*cosElev), 1/(4*cosElev), 1/(4*cosElev)} }; + + // make direction lines + directions = {FVector(sinElev, 0, -cosElev), + FVector(0, -sinElev, -cosElev), + FVector(-sinElev, 0, -cosElev), + FVector(0, sinElev, -cosElev)}; + // scale up to point MaxRange distance + for(FVector& d : directions) d *= MaxRange; +} + +void UDVLSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the velocity and buffer, then send the data to it. + if (Parent != nullptr && bOn) { + float* FloatBuffer = static_cast(Buffer); + FTransform SensortoWorld = this->GetComponentTransform(); + FVector Location = this->GetComponentLocation(); + + //UE4 gives us world velocity, rotate it to get local velocity + FVector Velocity = Parent->GetPhysicsLinearVelocityAtPoint(this->GetComponentLocation()); + Velocity = SensortoWorld.GetRotation().UnrotateVector(Velocity); + Velocity = ConvertLinearVector(Velocity, UEToClient); + + // Add noise if it's been enabled + if(mvnVel.isUncertain()){ + TArray sample = mvnVel.sampleTArray(); + for(int i=0;i<4;i++){ + Velocity.X += transform[0][i]*sample[i]; + Velocity.Y += transform[1][i]*sample[i]; + Velocity.Z += transform[2][i]*sample[i]; + + } + } + + // Send to buffer + FloatBuffer[0] = Velocity.X; + FloatBuffer[1] = Velocity.Y; + FloatBuffer[2] = Velocity.Z; + + + + // Get range if it was requested + if(ReturnRange){ + // Get parameters we'll need + FCollisionQueryParams QueryParams = FCollisionQueryParams(); + QueryParams.AddIgnoredActor(this->GetAttachmentRootActor()); + + // iterate through and do all raytracing + for(int i=0;i<4;i++){ + FVector end = SensortoWorld.TransformPositionNoScale(directions[i]); + FHitResult Hit = FHitResult(); + bool TraceResult = GetWorld()->LineTraceSingleByChannel(Hit, Location, end, ECollisionChannel::ECC_Visibility, QueryParams); + FloatBuffer[i+3] = (TraceResult ? Hit.Distance : MaxRange) / 100; // centimeter to meters + } + + // Add noise if it's been enabled + if(mvnRange.isUncertain()){ + TArray sample = mvnRange.sampleTArray(); + for(int i=0;i<4;i++){ + FloatBuffer[i+3] += sample[i]; + } + } + } + + + + // display debug lines if we want + if(DebugLines){ + for(int i=0;i<4;i++){ + FVector end = SensortoWorld.TransformPositionNoScale(directions[i]); + DrawDebugLine(GetWorld(), Location, end, FColor::Green, false, .01, ECC_WorldStatic, 1.f); + } + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DepthSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DepthSensor.cpp new file mode 100644 index 0000000000..2c144dd542 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/DepthSensor.cpp @@ -0,0 +1,49 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "DepthSensor.h" + +UDepthSensor::UDepthSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "DepthSensor"; +} + +void UDepthSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Sigma")) { + mvn.initSigma(JsonParsed->GetNumberField("Sigma")); + } + if (JsonParsed->HasTypedField("Cov")) { + mvn.initCov(JsonParsed->GetNumberField("Cov")); + } + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UDepthSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void UDepthSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object you are attached to. + Parent = this->GetAttachParent(); +} + +void UDepthSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the location and buffer, then send the location to the buffer. + if (Parent != nullptr && bOn) { + float* FloatBuffer = static_cast(Buffer); + + FVector Location = this->GetComponentLocation(); + Location = ConvertLinearVector(Location, UEToClient); + + Location.Z += mvn.sampleFloat(); + FloatBuffer[0] = Location.Z; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/GPSSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/GPSSensor.cpp new file mode 100644 index 0000000000..b90ac09fe7 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/GPSSensor.cpp @@ -0,0 +1,81 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "GPSSensor.h" + +UGPSSensor::UGPSSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "GPSSensor"; +} + +void UGPSSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Sigma")) { + mvn.initSigma(JsonParsed->GetNumberField("Sigma")); + } + if (JsonParsed->HasTypedField("Sigma")) { + mvn.initSigma(JsonParsed->GetArrayField("Sigma")); + } + + if (JsonParsed->HasTypedField("Cov")) { + mvn.initCov(JsonParsed->GetNumberField("Cov")); + } + if (JsonParsed->HasTypedField("Cov")) { + mvn.initCov(JsonParsed->GetArrayField("Cov")); + } + + if (JsonParsed->HasTypedField("Depth")) { + // Take absolute value in case people put it in as a negative (ie depth is down) + GPSDepth = FMath::Abs(JsonParsed->GetNumberField("Depth")); + } + + if (JsonParsed->HasTypedField("DepthSigma")) { + depthMVN.initSigma(JsonParsed->GetNumberField("DepthSigma")); + } + if (JsonParsed->HasTypedField("DepthCov")) { + depthMVN.initCov(JsonParsed->GetNumberField("DepthCov")); + } + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UGPSSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void UGPSSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object you are attached to. + Parent = this->GetAttachParent(); +} + +void UGPSSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the location and buffer, then send the location to the buffer. + if (Parent != nullptr && bOn) { + FVector Location = this->GetComponentLocation(); + + // Get location + float* FloatBuffer = static_cast(Buffer); + Location = ConvertLinearVector(Location, UEToClient); + + // Check to make sure we're not any deeper than depth + float depth = -1*Location.Z + depthMVN.sampleFloat(); + if(depth < GPSDepth){ + Location += mvn.sampleFVector(); + FloatBuffer[0] = Location.X; + FloatBuffer[1] = Location.Y; + FloatBuffer[2] = Location.Z; + } + // If we are, we get nothing back + else{ + FloatBuffer[0] = std::numeric_limits::quiet_NaN(); + FloatBuffer[1] = std::numeric_limits::quiet_NaN(); + FloatBuffer[2] = std::numeric_limits::quiet_NaN(); + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCamera.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCamera.cpp new file mode 100644 index 0000000000..c5e5deb944 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCamera.cpp @@ -0,0 +1,72 @@ +#include "Holodeck.h" +#include "HolodeckCamera.h" +#include "Json.h" + +UHolodeckCamera::UHolodeckCamera() { + UE_LOG(LogHolodeck, Log, TEXT("UHolodeckCamera::UHolodeckCamer() initialization called.")); + +} + +// Allows sensor parameters to be set programmatically from client. +void UHolodeckCamera::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("CaptureWidth")) { + CaptureWidth = JsonParsed->GetIntegerField("CaptureWidth"); + } + + if (JsonParsed->HasTypedField("CaptureHeight")) { + CaptureHeight = JsonParsed->GetIntegerField("CaptureHeight"); + } + } else { + UE_LOG(LogHolodeck, Fatal, TEXT("UHolodeckCamera::ParseSensorParms:: Unable to parse json.")); + } + + UE_LOG(LogHolodeck, Log, TEXT("CaptureHeight is %d"), CaptureHeight); + UE_LOG(LogHolodeck, Log, TEXT("CaptureWidth is %d"), CaptureWidth); + +} + +void UHolodeckCamera::InitializeSensor() { + UE_LOG(LogHolodeck, Log, TEXT("UHolodeckCamera::InitializeSensor")); + Super::InitializeSensor(); + + SceneCapture = NewObject(this, "SceneCap"); + SceneCapture->RegisterComponent(); + SceneCapture->AttachToComponent(this, FAttachmentTransformRules::KeepRelativeTransform); + + TargetTexture = NewObject(this); + + RenderRequest = FRenderRequest(); + + //set up everything for the texture that you are using for output. These won't likely change for subclasses. + //Note: the format should probably be 512 x 512 because it must be a square shape, and a power of two. If not, then the TargetTexture will cause crashes. + TargetTexture->SRGB = false; //No alpha + TargetTexture->CompressionSettings = TC_VectorDisplacementmap; + TargetTexture->RenderTargetFormat = RTF_RGBA8; + TargetTexture->InitCustomFormat(CaptureWidth, CaptureHeight, PF_FloatRGBA, false); + + //Handle whatever setup of the SceneCapture that won't likely change across different cameras. (These will be the defaults) + SceneCapture->ProjectionType = ECameraProjectionMode::Perspective; + SceneCapture->CompositeMode = SCCM_Overwrite; + SceneCapture->FOVAngle = 90; //90 degrees for field of view. + SceneCapture->CaptureSource = SCS_SceneColorHDR; + SceneCapture->TextureTarget = TargetTexture; + SceneCapture->PostProcessSettings.bOverride_AutoExposureBias = 1; + + // Higher = brighter captured image. Lower = darker + // This is a magic number that has been fine tuned to the default worlds. Do not edit without thourough testing. + SceneCapture->PostProcessSettings.AutoExposureBias = 4; + + //The buffer has got to be an FColor pointer so you can export the pixel data to it. + this->Buffer = static_cast(Super::Buffer); + this->ViewportClient = Cast(GEngine->GameViewport); +} + +void UHolodeckCamera::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + RenderRequest.RetrievePixels(Buffer, TargetTexture); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCollisionSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCollisionSensor.cpp new file mode 100644 index 0000000000..a5d2769c8c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/HolodeckCollisionSensor.cpp @@ -0,0 +1,36 @@ +#include "Holodeck.h" +#include "HolodeckCollisionSensor.h" + +UHolodeckCollisionSensor::UHolodeckCollisionSensor() { + PrimaryComponentTick.bCanEverTick = true; + bWantsInitializeComponent = true; + SensorName = "HolodeckCollisionSensor"; +} + +void UHolodeckCollisionSensor::InitializeComponent() { + Super::InitializeComponent(); + + Parent = this->GetOwner(); + //Set up the hit delegate, then give it to the parent. The parent will then call OnHit whenever it collides. + FScriptDelegate HitDelegate; + HitDelegate.BindUFunction(this, TEXT("OnHit")); + if (Parent) { + Parent->OnActorHit.AddUnique(HitDelegate); + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UHolodeckCollisionSensor::Parent was never initialized. Cannot add HitDelegate")); + } +} + +void UHolodeckCollisionSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //Update the buffer to current collision status, then set the colliding bool to false. + if (Parent != nullptr && bOn) { + bool* BoolBuffer = static_cast(Buffer); + BoolBuffer[0] = bIsColliding; + } + bIsColliding = false; +} + +void UHolodeckCollisionSensor::OnHit(AActor* SelfActor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit) { + bIsColliding = true; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/IMUSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/IMUSensor.cpp new file mode 100644 index 0000000000..9046328b74 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/IMUSensor.cpp @@ -0,0 +1,167 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "IMUSensor.h" + +UIMUSensor::UIMUSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "IMUSensor"; +} + +void UIMUSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("ReturnBias")) { + ReturnBias = JsonParsed->GetBoolField("ReturnBias"); + } + + // Acceleration noise + if (JsonParsed->HasTypedField("AccelSigma")) { + mvnAccel.initSigma(JsonParsed->GetNumberField("AccelSigma")); + } + if (JsonParsed->HasTypedField("AccelSigma")) { + mvnAccel.initSigma(JsonParsed->GetArrayField("AccelSigma")); + } + if (JsonParsed->HasTypedField("AccelCov")) { + mvnAccel.initCov(JsonParsed->GetNumberField("AccelCov")); + } + if (JsonParsed->HasTypedField("AccelCov")) { + mvnAccel.initCov(JsonParsed->GetArrayField("AccelCov")); + } + + // Angular Velocity noise + if (JsonParsed->HasTypedField("AngVelSigma")) { + mvnOmega.initSigma(JsonParsed->GetNumberField("AngVelSigma")); + } + if (JsonParsed->HasTypedField("AngVelSigma")) { + mvnOmega.initSigma(JsonParsed->GetArrayField("AngVelSigma")); + } + if (JsonParsed->HasTypedField("AngVelCov")) { + mvnOmega.initCov(JsonParsed->GetNumberField("AngVelCov")); + } + if (JsonParsed->HasTypedField("AngVelCov")) { + mvnOmega.initCov(JsonParsed->GetArrayField("AngVelCov")); + } + + // Acceleration Bias noise + if (JsonParsed->HasTypedField("AccelBiasSigma")) { + mvnBiasAccel.initSigma(JsonParsed->GetNumberField("AccelBiasSigma")); + } + if (JsonParsed->HasTypedField("AccelBiasSigma")) { + mvnBiasAccel.initSigma(JsonParsed->GetArrayField("AccelBiasSigma")); + } + if (JsonParsed->HasTypedField("AccelBiasCov")) { + mvnBiasAccel.initCov(JsonParsed->GetNumberField("AccelBiasCov")); + } + if (JsonParsed->HasTypedField("AccelBiasCov")) { + mvnBiasAccel.initCov(JsonParsed->GetArrayField("AccelBiasCov")); + } + + // Angular Velocity noise + if (JsonParsed->HasTypedField("AngVelBiasSigma")) { + mvnBiasOmega.initSigma(JsonParsed->GetNumberField("AngVelBiasSigma")); + } + if (JsonParsed->HasTypedField("AngVelBiasSigma")) { + mvnBiasOmega.initSigma(JsonParsed->GetArrayField("AngVelBiasSigma")); + } + if (JsonParsed->HasTypedField("AngVelBiasCov")) { + mvnBiasOmega.initCov(JsonParsed->GetNumberField("AngVelBiasCov")); + } + if (JsonParsed->HasTypedField("AngVelBiasCov")) { + mvnBiasOmega.initCov(JsonParsed->GetArrayField("AngVelBiasCov")); + } + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UIMUSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void UIMUSensor::InitializeSensor() { + Super::InitializeSensor(); + + // Cache important variables + Parent = Cast(this->GetAttachParent()); + + World = Parent->GetWorld(); + WorldSettings = World->GetWorldSettings(false, false); + WorldGravity = WorldSettings->GetGravityZ(); + + VelocityThen = FVector(); + VelocityNow = FVector(); + LinearAccelerationVector = FVector(); + AngularVelocityVector = FVector(); +} + + +void UIMUSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction * ThisTickFunction) { + if (Parent != nullptr && bOn) { + CalculateAccelerationVector(DeltaTime); + CalculateAngularVelocityVector(); + + float* FloatBuffer = static_cast(Buffer); + + // Convert before sending to user side. + LinearAccelerationVector = ConvertLinearVector(LinearAccelerationVector, UEToClient); + AngularVelocityVector = ConvertAngularVector(AngularVelocityVector, NoScale); + + // Introduce noise + BiasAccel += mvnBiasAccel.sampleFVector(); + BiasOmega += mvnBiasOmega.sampleFVector(); + LinearAccelerationVector += BiasAccel + mvnAccel.sampleFVector(); + AngularVelocityVector += BiasOmega + mvnOmega.sampleFVector(); + + FloatBuffer[0] = LinearAccelerationVector.X; + FloatBuffer[1] = LinearAccelerationVector.Y; + FloatBuffer[2] = LinearAccelerationVector.Z; + FloatBuffer[3] = AngularVelocityVector.X; + FloatBuffer[4] = AngularVelocityVector.Y; + FloatBuffer[5] = AngularVelocityVector.Z; + + if(ReturnBias){ + FloatBuffer[6] = BiasAccel.X; + FloatBuffer[7] = BiasAccel.Y; + FloatBuffer[8] = BiasAccel.Z; + FloatBuffer[9] = BiasOmega.X; + FloatBuffer[10] = BiasOmega.Y; + FloatBuffer[11] = BiasOmega.Z; + } + } +} + +void UIMUSensor::CalculateAccelerationVector(float DeltaTime) { + VelocityThen = VelocityNow; + VelocityNow = Parent->GetPhysicsLinearVelocityAtPoint(this->GetComponentLocation()); + + RotationNow = this->GetComponentRotation(); + + LinearAccelerationVector = VelocityNow - VelocityThen; + LinearAccelerationVector /= DeltaTime; + + LinearAccelerationVector += FVector(0.0, 0.0, -WorldGravity); + + LinearAccelerationVector = RotationNow.UnrotateVector(LinearAccelerationVector); //changes world axis to local axis +} + +void UIMUSensor::CalculateAngularVelocityVector() { + AngularVelocityVector = Parent->GetPhysicsAngularVelocityInRadians(); + + AngularVelocityVector.X = AngularVelocityVector.X; + AngularVelocityVector.Y = AngularVelocityVector.Y; + AngularVelocityVector.Z = AngularVelocityVector.Z; + + AngularVelocityVector = RotationNow.UnrotateVector(AngularVelocityVector); //Rotate from world angles to local angles. + +} + +FVector UIMUSensor::GetAccelerationVector() { + return LinearAccelerationVector; +} + +FVector UIMUSensor::GetAngularVelocityVector() { + return AngularVelocityVector; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ImagingSonar.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ImagingSonar.cpp new file mode 100644 index 0000000000..66c3399dd3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ImagingSonar.cpp @@ -0,0 +1,425 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "Benchmarker.h" +#include "HolodeckBuoyantAgent.h" +#include "ImagingSonar.h" +// #pragma warning (disable : 4101) + +UImagingSonar::UImagingSonar() { + SensorName = "ImagingSonar"; +} + +void UImagingSonar::BeginDestroy() { + Super::BeginDestroy(); + + delete[] count; + delete[] hasPerfectNormal; +} + +// Allows sensor parameters to be set programmatically from client. +void UImagingSonar::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + // For handling noise + if (JsonParsed->HasTypedField("AddSigma")) { + addNoise.initSigma(JsonParsed->GetNumberField("AddSigma")); + } + if (JsonParsed->HasTypedField("AddCov")) { + addNoise.initCov(JsonParsed->GetNumberField("AddCov")); + } + if (JsonParsed->HasTypedField("MultSigma")) { + multNoise.initSigma(JsonParsed->GetNumberField("MultSigma")); + } + if (JsonParsed->HasTypedField("MultCov")) { + multNoise.initCov(JsonParsed->GetNumberField("MultCov")); + } + if (JsonParsed->HasTypedField("RangeSigma")) { + rNoise.initBounds(JsonParsed->GetNumberField("RangeSigma")*100); + } + if (JsonParsed->HasTypedField("ScaleNoise")) { + ScaleNoise = JsonParsed->GetBoolField("ScaleNoise"); + } + if (JsonParsed->HasTypedField("AzimuthStreaks")) { + AzimuthStreaks = JsonParsed->GetIntegerField("AzimuthStreaks"); + } + + // Multipath Settings + if (JsonParsed->HasTypedField("MultiPath")) { + MultiPath = JsonParsed->GetBoolField("MultiPath"); + } + if (JsonParsed->HasTypedField("ClusterSize")) { + ClusterSize = JsonParsed->GetIntegerField("ClusterSize"); + } + + // Size of our binning + if (JsonParsed->HasTypedField("RangeBins")) { + RangeBins = JsonParsed->GetIntegerField("RangeBins"); + } + if (JsonParsed->HasTypedField("RangeRes")) { + RangeRes = JsonParsed->GetNumberField("RangeRes")*100; + } + if (JsonParsed->HasTypedField("AzimuthBins")) { + AzimuthBins = JsonParsed->GetIntegerField("AzimuthBins"); + } + if (JsonParsed->HasTypedField("AzimuthRes")) { + AzimuthRes = JsonParsed->GetNumberField("AzimuthRes"); + } + if (JsonParsed->HasTypedField("ElevationBins")) { + ElevationBins = JsonParsed->GetIntegerField("ElevationBins"); + } + if (JsonParsed->HasTypedField("ElevationRes")) { + ElevationRes = JsonParsed->GetNumberField("ElevationRes"); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UImagingSonar::ParseSensorParms:: Unable to parse json.")); + } + + // Parse through the Range parameters given to us + if(RangeBins != 0){ + RangeRes = (RangeMax - RangeMin) / RangeBins; + } + else if(RangeRes != 0){ + RangeBins = (RangeMax - RangeMin) / RangeRes; + } + else{ + RangeBins = 512; + RangeRes = (RangeMax - RangeMin) / RangeBins; + } + + // Parse through the Azimuth parameters given to us + if(AzimuthBins != 0){ + AzimuthRes = Azimuth / AzimuthBins; + } + else if(AzimuthRes != 0){ + AzimuthBins = Azimuth / AzimuthRes; + } + else{ + AzimuthBins = 512; + AzimuthRes = Azimuth / AzimuthBins; + } + + // Parse through the Elevation parameters given to us + if(ElevationBins != 0){ + ElevationRes = Elevation / ElevationBins; + } + else if(ElevationRes != 0){ + ElevationBins = Elevation / ElevationRes; + } + else{ + // Calculate how large our shadowing bins should be + float dist = (RangeMax - RangeMin) / 8 + RangeMin; + ElevationBins = (dist*Elevation*Pi/180) / Octree::OctreeMin; + if(ElevationBins < 1) ElevationBins = 1; + ElevationRes = Elevation / ElevationBins; + } +} + +void UImagingSonar::InitializeSensor() { + Super::InitializeSensor(); + + // Check if we should shadow with less Azimuth bins + float dist = (RangeMax - RangeMin) / 8 + RangeMin; + AzimuthBinScale = 1; + while(Octree::OctreeMin >= (dist*AzimuthRes*Pi/180)*AzimuthBinScale){ + AzimuthBinScale *= 2; + } + if(AzimuthBinScale > AzimuthBins) AzimuthBinScale = AzimuthBins; + + // setup count of each bin + count = new int32[RangeBins*AzimuthBins](); + hasPerfectNormal = new int32[AzimuthBins*RangeBins](); + + // Define a perfect reflection + perfectCos = UKismetMathLibrary::DegCos(8); + for(int i=0;i()); + sortedLeaves[i].Reserve(10000); + } + + mapLeaves.Reserve(100000); +} + + +void UImagingSonar::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + Super::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); + + if(TickCounter == 0){ + // reset things and get ready + float* result = static_cast(Buffer); + std::fill(result, result+RangeBins*AzimuthBins, 0); + std::fill(count, count+RangeBins*AzimuthBins, 0); + std::fill(hasPerfectNormal, hasPerfectNormal+AzimuthBins*RangeBins, 0); + + for(auto& sl: sortedLeaves){ + sl.Reset(); + } + mapLeaves.Reset(); + mapSearch.Reset(); + cluster.Reset(); + + + // Finds leaves in range and puts them in foundLeaves + findLeaves(); + + // SORT THEM INTO AZIMUTH/ELEVATION BINS + int32 idx; + for(TArray& bin : foundLeaves){ + for(Octree* l : bin){ + // Compute bins while we're parallelized + l->idx.Y = (int32)((l->locSpherical.Y - minAzimuth)/ AzimuthRes); + l->idx.Z = (int32)((l->locSpherical.Z - minElev)/ ElevationRes); + // Sometimes we get float->int rounding errors + if(l->idx.Y == AzimuthBins) --l->idx.Y; + + idx = l->idx.Z*AzimuthBins/AzimuthBinScale + l->idx.Y/AzimuthBinScale; + sortedLeaves[idx].Emplace(l); + } + } + + // HANDLE SHADOWING + shadowLeaves(); + + // ADD IN ALL CONTRIBUTIONS + float noise, pdf; + for(TArray& bin : sortedLeaves){ + for(Octree* l : bin){ + // Add noise to each of them + noise = rNoise.sampleExponential(); + pdf = rNoise.exponentialScaledPDF(noise); + l->idx.X = (int32)((l->locSpherical.X + noise - RangeMin) / RangeRes); + l->val *= pdf; + + // In case our noise has pushed us out of range + if(l->idx.X >= RangeBins) l->idx.X = RangeBins-1; + + // Add to their appropriate bin + idx = l->idx.X*AzimuthBins + l->idx.Y; + if(l->cos > perfectCos) hasPerfectNormal[idx] += 1; + + result[idx] += l->val; + ++count[idx]; + } + } + + if(MultiPath){ + // PUT INTO MAP FOR CLUSTER + for(TArray& binLeafs : sortedLeaves){ + if(binLeafs.Num() > 0){ + // Get first element in this azimuth, elevation bin (ie idx.Y and idx.Z are the same for all of these) + Octree* jth = binLeafs.GetData()[0]; + mapLeaves.Add(jth->idx, jth); + int idxR = jth->idx.X; + // Iterate through only taking ones with different range idx (idx.X) + // Note that the bin is sorted from shadowing above. + for(int i=1;iidx.X != idxR){ + mapLeaves.Add(jth->idx, jth); + idxR = jth->idx.X; + } + } + } + } + + // PUT THEM INTO CLUSTERS + mapSearch = TMap(mapLeaves); + mapSearch.Compact(); + int i_start, j_start, k_start, i_end, j_end, k_end; + Octree** close = nullptr; + while(mapSearch.Num() > 0){ + // Get start of cluster + Octree* l = mapSearch.begin()->Value; + mapSearch.Remove(l->idx); + cluster.Add({l}); + + // Get anything that may be nearby + i_start = FGenericPlatformMath::Max(0,l->idx.X-ClusterSize); + j_start = FGenericPlatformMath::Max(0,l->idx.Y-ClusterSize); + k_start = FGenericPlatformMath::Max(0,l->idx.Z-ClusterSize); + i_end = FGenericPlatformMath::Min(RangeBins,l->idx.X+ClusterSize+1); + j_end = FGenericPlatformMath::Min(AzimuthBins,l->idx.Y+ClusterSize+1); + k_end = FGenericPlatformMath::Min(ElevationBins,l->idx.Z+ClusterSize+1); + for(int i=i_start; inormal, (*close)->normal) > 0.965){ + cluster.Top().Add(*close); + mapSearch.Remove((*close)->idx); + } + } + } + } + } + + + // MULTIPATH CONTRIBUTIONS + float step_size = Octree::OctreeMin; + int iterations = RangeMax / Octree::OctreeMin; + FTransform SensortoWorld = this->GetComponentTransform(); + std::function reflect; + reflect = [](FVector normal, FVector impact){ + return -impact + 2*FVector::DotProduct(normal,impact)*normal; + }; + ParallelFor(cluster.Num(), [&](int32 i){ + TArray& thisCluster = cluster.GetData()[i]; + Octree* l = thisCluster.GetData()[0]; + + FVector reflection = reflect(l->normal, l->normalImpact); + Octree stepper(l->loc, l->size); + Octree** hit = nullptr; + FVector offset = reflection*step_size*30; + + // TODO: Replace this with real raytracing? + for(int32 j=0;jloc + offset; + + // make sure it's still in range (& compute spherical coordinates) + if(!inRange(&stepper)){ + thisCluster.Empty(); + return; + } + + // Set the index values + stepper.idx.X = (int32)((stepper.locSpherical.X - RangeMin) / RangeRes); + stepper.idx.Y = (int32)((stepper.locSpherical.Y - minAzimuth)/ AzimuthRes); + stepper.idx.Z = (int32)((stepper.locSpherical.Z - minElev)/ ElevationRes); + + // If there's something in that bin + hit = mapLeaves.Find(stepper.idx); + if(hit != nullptr){ + FVector returnImpact = reflect((*hit)->normal, -reflection); + // make sure it's in the right direction + if(FVector::DotProduct(returnImpact, (*hit)->normalImpact) > 0) break; + else { + thisCluster.Empty(); + return; + } + } + } + + if(hit == nullptr){ + thisCluster.Empty(); + return; + } + + // If we did hit something, ray trace the rest of everything in the cluster + float t, noise, pdf, R1, R2; + FVector locBounce, returnRay; + for(Octree* m : thisCluster){ + // find 2nd impact location + reflection = reflect(m->normal, m->normalImpact); + t = FVector::DotProduct((*hit)->loc - m->loc, (*hit)->normal) / (FVector::DotProduct(reflection, (*hit)->normal)); + locBounce = m->loc + reflection*t; + + // find return vector + // TODO: See if any change in accuracy in just using the hit version, should be pretty close angles + + // find ray return + returnRay = reflect((*hit)->normal, -reflection); + + // find spherical location + Octree bounce(locBounce, m->size); + inRange(&bounce); + // float dist = bounce.locSpherical.X; + bounce.locSpherical.X += m->locSpherical.X + FVector::Dist(bounce.loc, m->loc); + bounce.locSpherical.X /= 2; + + // Convert to contribution index + noise = rNoise.sampleExponential(); + pdf = rNoise.exponentialScaledPDF(noise); + m->idx.X = (int32)((bounce.locSpherical.X + noise - RangeMin) / RangeRes); + m->idx.Y = (int32)((bounce.locSpherical.Y - minAzimuth)/ AzimuthRes); + m->cos = FVector::DotProduct(returnRay, (*hit)->normalImpact); + R1 = (m->z - WaterImpedance) / (m->z + WaterImpedance); + R2 = ((*hit)->z - WaterImpedance) / ((*hit)->z + WaterImpedance); + m->val = R1*R1*R2*R2*m->cos*pdf; + + // TODO: There's a bug this is working around, find it and fix it + if(m->idx.X < 0) m->idx.X = 0; + if(m->idx.X > RangeBins) m->idx.X = RangeBins-1; + if(m->idx.Y < 0) m->idx.Y = 0; + if(m->idx.Y > AzimuthBins) m->idx.Y = AzimuthBins-1; + + // DrawDebugPoint(GetWorld(), m->loc, 3, FColor::Red, false, DeltaTime*TicksPerCapture); + // DrawDebugPoint(GetWorld(), bounce.loc, 3, FColor::Blue, false, DeltaTime*TicksPerCapture); + } + }, false); + + // ADD IN MULTIPATH CONTRIBUTIONS + for(TArray& bin : cluster){ + for(Octree* l : bin){ + idx = l->idx.X*AzimuthBins + l->idx.Y; + + result[idx] += l->val; + ++count[idx]; + } + } + } + + + // NORMALIZE & PERTURB RESULTS + float scale_range, scale_total, azimuth; + float std = Azimuth/64; + for (int i=0; i= percToBand){ + for(int j=0; jParent = this->GetOwner(); + + if (this->Parent->IsA(AAndroid::StaticClass())) { + this->Joints.Append(AAndroid::Joints, AAndroid::NUM_JOINTS); + + this->TotalDof = AAndroid::TOTAL_DOF; + this->Num3AxisJoints = AAndroid::NUM_3_AXIS_JOINTS; + this->Num2AxisJoints = AAndroid::NUM_2_AXIS_JOINTS; + } + else if (this->Parent->IsA(AHandAgent::StaticClass())) { + this->Joints.Append(AHandAgent::Joints, AHandAgent::NUM_JOINTS); + + this->TotalDof = AHandAgent::TOTAL_JOINT_DOF; + this->Num3AxisJoints = AHandAgent::NUM_3_AXIS_JOINTS; + this->Num2AxisJoints = AHandAgent::NUM_2_AXIS_JOINTS; + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("Error: Tried to use UJointRotationSensor with unknown agent type.")); + } + + TArray Components; + this->Parent->GetComponents(Components); + this->SkeletalMeshComponent = Components[0]; + + Super::InitializeSensor(); +} + +int UJointRotationSensor::GetNumItems() { + return this->TotalDof; +} + +void UJointRotationSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + float* FloatBuffer = static_cast(Buffer); + + for (int JointInd = 0; JointInd < this->Joints.Num(); JointInd++) { + + FString JointName = Joints[JointInd].ToString(); + + FloatBuffer = AddJointRotationToBuffer(JointName, true, + JointInd < this->Num2AxisJoints + this->Num3AxisJoints, + JointInd < this->Num3AxisJoints, + FloatBuffer); + } +} + +float* UJointRotationSensor::AddJointRotationToBuffer(FString JointName, bool Swing1, bool Swing2, bool Twist, float* Data) { + FConstraintInstance* Constraint = SkeletalMeshComponent->FindConstraintInstance(FName(*JointName)); + + if (Swing1) { + *Data = Constraint->GetCurrentSwing1(); + Data += 1; // pointer arithmetic, fun + } + if (Swing2) { + *Data = Constraint->GetCurrentSwing2(); + Data += 1; + } + if (Twist) { + *Data = Constraint->GetCurrentTwist(); + Data += 1; + } + + return Data; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/LocationSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/LocationSensor.cpp new file mode 100644 index 0000000000..df62868b02 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/LocationSensor.cpp @@ -0,0 +1,54 @@ +#include "Holodeck.h" +#include "LocationSensor.h" + +ULocationSensor::ULocationSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "LocationSensor"; +} + +void ULocationSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Sigma")) { + mvn.initSigma(JsonParsed->GetNumberField("Sigma")); + } + if (JsonParsed->HasTypedField("Sigma")) { + mvn.initSigma(JsonParsed->GetArrayField("Sigma")); + } + + if (JsonParsed->HasTypedField("Cov")) { + mvn.initCov(JsonParsed->GetNumberField("Cov")); + } + if (JsonParsed->HasTypedField("Cov")) { + mvn.initCov(JsonParsed->GetArrayField("Cov")); + } + + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("ULocationSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void ULocationSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object you are attached to. + Parent = this->GetAttachParent(); +} + +void ULocationSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the location and buffer, then send the location to the buffer. + if (Parent != nullptr && bOn) { + FVector Location = this->GetComponentLocation(); + float* FloatBuffer = static_cast(Buffer); + Location = ConvertLinearVector(Location, UEToClient); + Location += mvn.sampleFVector(); + FloatBuffer[0] = Location.X; + FloatBuffer[1] = Location.Y; + FloatBuffer[2] = Location.Z; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OpticalModemSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OpticalModemSensor.cpp new file mode 100644 index 0000000000..1c918430f5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OpticalModemSensor.cpp @@ -0,0 +1,177 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "OpticalModemSensor.h" +#include "Json.h" + +UOpticalModemSensor::UOpticalModemSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "OpticalModemSensor"; +} + +void UOpticalModemSensor::InitializeSensor() { + Super::InitializeSensor(); + //You need to get the pointer to the object the sensor is attached to. + Parent = this->GetAttachmentRootActor(); + NoiseMaxDistance = MaxDistance + DistanceNoise.sampleFloat(); + NoiseLaserAngle = LaserAngle + AngleNoise.sampleFloat(); +} + +void UOpticalModemSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the buffer before sending the data to it. + bool* BoolBuffer = static_cast(Buffer); + BoolBuffer[0] = false; + + if (Parent != nullptr && bOn) { + NoiseMaxDistance = MaxDistance + DistanceNoise.sampleFloat(); + NoiseLaserAngle = LaserAngle + AngleNoise.sampleFloat(); + if (FromSensor) { + // if someone starting transmitting and we're in range, we'll receive + BoolBuffer[0] = this->CanTransmit(); + + // Reset FromSensor now that the message has been sent + FromSensor = NULL; + } + } + + + if (LaserDebug) { + DrawDebugCone(GetWorld(), GetComponentLocation(), GetForwardVector(), NoiseMaxDistance * 100, FMath::DegreesToRadians(NoiseLaserAngle), FMath::DegreesToRadians(NoiseLaserAngle), DebugNumSides, DebugColor, false, .01, ECC_WorldStatic, 1.F); + // DrawDebugLine(GetWorld(), GetComponentLocation(), GetForwardVector() * NoiseMaxDistance * 100, DebugColor, false, .01,ECC_WorldStatic, 1.F); + } +} + +bool UOpticalModemSensor::CanTransmit() { + + // get coordinates of other sensor in local frame + FVector SendingSensor = this->GetComponentLocation(); + FVector ReceiveSensor = FromSensor->GetComponentLocation(); + FVector SendToReceive = ReceiveSensor - SendingSensor; + FVector ReceiveToSend = SendingSensor - ReceiveSensor; + + float Dist = SendToReceive.Size() / 100; + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Dist = %f MaxDistance = %f"), Dist, FromSensor->NoiseMaxDistance); + + bool transmit; + + //Max guaranteed range of modem is 50 meters + if (Dist <= FromSensor->NoiseMaxDistance) { + + // Calculate if sensors are facing each other within 120 degrees + //--> Difference in angle needs to be -60 < x < 60 + //--> Check both sensors to make sure both are in acceptable orientations + + if (IsSensorOriented(this, SendToReceive) && IsSensorOriented(FromSensor, ReceiveToSend)) { + // Calculate if rangefinder and dist are equal or not. + + FCollisionQueryParams QueryParams = FCollisionQueryParams(); + QueryParams.AddIgnoredActor(Parent); + + FHitResult Hit = FHitResult(); + + bool TraceResult = GetWorld()->LineTraceSingleByChannel(Hit, SendingSensor, ReceiveSensor, ECollisionChannel::ECC_Visibility, QueryParams); + + bool Range = (TraceResult && Hit.GetActor() == FromSensor->Parent); + + if (Range) { + // return true; + transmit = true; + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Transmit success")); + + } + else { + transmit = false; + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Transmit failed due to range")); + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Range = %f"), Hit.Distance); + + } + } + else{ + transmit = false; + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Transmit failed due to orientation")); + } + } + else{ + transmit = false; + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: Transmit failed due to distance")); + + } + return transmit; + //return false; +} + + +bool UOpticalModemSensor::IsSensorOriented(UOpticalModemSensor* Sensor, FVector LocalToSensor) { + float DotProduct = UKismetMathLibrary::Dot_VectorVector(UKismetMathLibrary::Normal(Sensor->GetForwardVector()), UKismetMathLibrary::Normal(LocalToSensor)); + float Angle = FMath::RadiansToDegrees(UKismetMathLibrary::Acos(DotProduct)); + UE_LOG(LogHolodeck, Log, TEXT("Optical Modem: angle = %f"), Angle); + + if (-1 * NoiseLaserAngle < Angle && Angle < NoiseLaserAngle) { + return true; + } + else { + return false; + } +} + +void UOpticalModemSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("MaxDistance")) { + MaxDistance = JsonParsed->GetIntegerField("MaxDistance"); + } + + if (JsonParsed->HasTypedField("LaserAngle")) { + LaserAngle = JsonParsed->GetIntegerField("LaserAngle"); + } + + if (JsonParsed->HasTypedField("DebugNumSides")) { + DebugNumSides = JsonParsed->GetIntegerField("DebugNumSides"); + } + + if (JsonParsed->HasTypedField("LaserDebug")) { + LaserDebug = JsonParsed->GetBoolField("LaserDebug"); + } + if (JsonParsed->HasTypedField("DebugColor")) { + FillColorMap(); + DebugColor = ColorMap[JsonParsed->GetStringField("DebugColor")]; + } + if (JsonParsed->HasTypedField("DistanceSigma")) { + DistanceNoise.initSigma(JsonParsed->GetNumberField("DistanceSigma")); + } + if (JsonParsed->HasTypedField("AngleSigma")) { + AngleNoise.initSigma(JsonParsed->GetNumberField("AngleSigma")); + } + if (JsonParsed->HasTypedField("DistanceCov")) { + DistanceNoise.initCov(JsonParsed->GetNumberField("DistanceCov")); + } + if (JsonParsed->HasTypedField("AngleCov")) { + AngleNoise.initCov(JsonParsed->GetNumberField("AngleCov")); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("URangeFinderSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void UOpticalModemSensor::FillColorMap() { + ColorMap.Add("Black", FColor::Black); + ColorMap.Add("Blue", FColor::Blue); + ColorMap.Add("Cyan", FColor::Cyan); + ColorMap.Add("Emerald", FColor::Emerald); + ColorMap.Add("Green", FColor::Green); + ColorMap.Add("Magenta", FColor::Magenta); + ColorMap.Add("Orange", FColor::Orange); + ColorMap.Add("Purple", FColor::Purple); + ColorMap.Add("Red", FColor::Red); + ColorMap.Add("Silver", FColor::Silver); + ColorMap.Add("Transparent", FColor::Transparent); + ColorMap.Add("Turquoise", FColor::Turquoise); + ColorMap.Add("White", FColor::White); + ColorMap.Add("Yellow", FColor::Yellow); + ColorMap.Add("Random", FColor::MakeRandomColor()); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OrientationSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OrientationSensor.cpp new file mode 100644 index 0000000000..b8fe90e67b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/OrientationSensor.cpp @@ -0,0 +1,43 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "OrientationSensor.h" + +UOrientationSensor::UOrientationSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "OrientationSensor"; +} + +void UOrientationSensor::InitializeSensor() { + Super::InitializeSensor(); + + Controller = static_cast(this->GetAttachmentRootActor()->GetInstigator()->Controller); + Parent = static_cast(this->GetAttachParent()); + RootMesh = static_cast(this->GetAttachParent()); + + World = Parent->GetWorld(); +} + +// Called every frame +void UOrientationSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (Parent != nullptr && RootMesh != nullptr) { + FVector Forward = this->GetForwardVector(); + FVector Right = this->GetRightVector(); + FVector Up = this->GetUpVector(); + + float* FloatBuffer = static_cast(Buffer); + Forward = ConvertLinearVector(Forward, NoScale); + FVector Left = ConvertLinearVector(-Right, NoScale); + Up = ConvertLinearVector(Up, NoScale); + + FloatBuffer[0] = Forward.X; + FloatBuffer[3] = Forward.Y; + FloatBuffer[6] = Forward.Z; + FloatBuffer[1] = Left.X; + FloatBuffer[4] = Left.Y; + FloatBuffer[7] = Left.Z; + FloatBuffer[2] = Up.X; + FloatBuffer[5] = Up.Y; + FloatBuffer[8] = Up.Z; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PoseSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PoseSensor.cpp new file mode 100644 index 0000000000..45452158bb --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PoseSensor.cpp @@ -0,0 +1,57 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "PoseSensor.h" + +UPoseSensor::UPoseSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "PoseSensor"; +} + +void UPoseSensor::InitializeSensor() { + Super::InitializeSensor(); + + Controller = static_cast(this->GetAttachmentRootActor()->GetInstigator()->Controller); + Parent = static_cast(this->GetAttachParent()); + RootMesh = static_cast(this->GetAttachParent()); + + World = Parent->GetWorld(); +} + +// Called every frame +void UPoseSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (Parent != nullptr && RootMesh != nullptr) { + FVector Forward = this->GetForwardVector(); + FVector Right = this->GetRightVector(); + FVector Up = this->GetUpVector(); + FVector Location = this->GetComponentLocation(); + + float* FloatBuffer = static_cast(Buffer); + Forward = ConvertLinearVector(Forward, NoScale); + FVector Left = ConvertLinearVector(-Right, NoScale); + Up = ConvertLinearVector(Up, NoScale); + Location = ConvertLinearVector(Location, UEToClient); + + // Insert Rotation Matrix + FloatBuffer[0] = Forward.X; + FloatBuffer[4] = Forward.Y; + FloatBuffer[8] = Forward.Z; + FloatBuffer[1] = Left.X; + FloatBuffer[5] = Left.Y; + FloatBuffer[9] = Left.Z; + FloatBuffer[2] = Up.X; + FloatBuffer[6] = Up.Y; + FloatBuffer[10] = Up.Z; + + // Insert Position + FloatBuffer[3] = Location.X; + FloatBuffer[7] = Location.Y; + FloatBuffer[11] = Location.Z; + + // Insert Bottom Row + FloatBuffer[12] = 0; + FloatBuffer[13] = 0; + FloatBuffer[14] = 0; + FloatBuffer[15] = 1; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PressureSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PressureSensor.cpp new file mode 100644 index 0000000000..f7f5eb17e1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/PressureSensor.cpp @@ -0,0 +1,106 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "PressureSensor.h" + +// Sets default values for this component's properties +UPressureSensor::UPressureSensor() { + // Set this component to be initialized when the game starts, and to be ticked every frame. You can turn these features + // off to improve performance if you don't need them. + PrimaryComponentTick.bCanEverTick = true; + SensorName = "PressureSensor"; +} + + +// Called when the game starts +void UPressureSensor::InitializeSensor() { + + this->Parent = this->GetOwner(); + + if (this->Parent->IsA(AAndroid::StaticClass())) { + this->NumJoints = AAndroid::NUM_JOINTS; + this->Joints = const_cast(AAndroid::Joints); + } + else if (this->Parent->IsA(AHandAgent::StaticClass())) { + // is AHandAgent + this->NumJoints = AHandAgent::NUM_JOINTS; + this->Joints = const_cast(AHandAgent::Joints); + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("Error: Tried to use UPressureSensor with unknown agent type.")); + } + + + InitJointMap(); + + this->PrivateData = new float[this->GetNumItems() * sizeof(float)]; + memset(PrivateData, 0, GetNumItems() * sizeof(float)); + + //set hit delegate + FScriptDelegate hitDelegate; + hitDelegate.BindUFunction(this, TEXT("OnHit")); + this->GetAttachmentRootActor()->OnActorHit.AddUnique(hitDelegate); + + //assign skeletal mesh component + TArray Components; + this->GetAttachmentRootActor()->GetComponents(Components); + SkeletalMeshComponent = Components[0]; + + Super::InitializeSensor(); +} + +void UPressureSensor::BeginDestroy() { + Super::BeginDestroy(); + + delete this->PrivateData; +} + +int UPressureSensor::GetNumItems() { + // 3 vectors containing impulse normal info and hit location + return this->NumJoints * (3 + 1); +} + +// Called every frame +void UPressureSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + float* FloatBuffer = static_cast(Buffer); + + memcpy(FloatBuffer, PrivateData, GetNumItems() * sizeof(float)); + memset(PrivateData, 0, GetNumItems() * sizeof(float)); +} + +void UPressureSensor::OnHit(AActor* SelfActor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit) { + FVector HitWorldLocation; + FVector HitBoneLocation; + FRotator HitBoneRotation; + + HitWorldLocation = Hit.Location; + FRotator HitWorldRotation = SkeletalMeshComponent->GetSocketRotation(Hit.BoneName); + + SkeletalMeshComponent->TransformToBoneSpace(Hit.BoneName, HitWorldLocation, HitWorldRotation, HitBoneLocation, HitBoneRotation); + + if(JointMap.Contains(Hit.BoneName.ToString())) + AddHitToBuffer(Hit.BoneName.ToString(), HitBoneLocation, NormalImpulse, PrivateData); +} + +float* UPressureSensor::AddHitToBuffer(FString BoneName,FVector HitBoneLocation, FVector NormalImpulse, float* Data) { + + int JointInd = JointMap[BoneName] * 4; + + HitBoneLocation = ConvertLinearVector(HitBoneLocation, UEToClient); + NormalImpulse = ConvertLinearVector(NormalImpulse, UEToClient); + + Data[JointInd] = HitBoneLocation.X; + Data[JointInd+1] = HitBoneLocation.Y; + Data[JointInd+2] = HitBoneLocation.Z; + Data[JointInd+3] = NormalImpulse.Size(); + + return Data; +} + +void UPressureSensor::InitJointMap() { + + for (int JointInd = 0; JointInd < this->NumJoints; JointInd++) { + FString Joint = this->Joints[JointInd].ToString(); + JointMap.Add(Joint, JointInd); + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ProfilingSonar.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ProfilingSonar.cpp new file mode 100644 index 0000000000..b3e732ea63 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ProfilingSonar.cpp @@ -0,0 +1,32 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "ProfilingSonar.h" +// #pragma warning (disable : 4101) + +UProfilingSonar::UProfilingSonar() { + SensorName = "ProfilingSonar"; +} + +// Allows sensor parameters to be set programmatically from client. +void UProfilingSonar::ParseSensorParms(FString ParmsJson) { + Elevation = 1; + RangeMin = 0.5 * 100; + RangeMax = 75 * 100; + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + // If they haven't set RangeRes or RangeBins, input our default RangeBins + if (!JsonParsed->HasTypedField("RangeRes") && !JsonParsed->HasTypedField("RangeBins")) { + RangeBins = 750; + } + + // If they haven't set AzimuthRes or AzimuthBins, input our default AzimuthBins + if (!JsonParsed->HasTypedField("AzimuthRes") && !JsonParsed->HasTypedField("AzimuthBins")) { + AzimuthBins = 480; + } + } + + Super::ParseSensorParms(ParmsJson); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RGBCamera.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RGBCamera.cpp new file mode 100644 index 0000000000..83e5810899 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RGBCamera.cpp @@ -0,0 +1,39 @@ +#include "Holodeck.h" +#include "RGBCamera.h" +#include "Json.h" + +URGBCamera::URGBCamera() { + SensorName = "RGBCamera"; +} + +// Allows sensor parameters to be set programmatically from client. +void URGBCamera::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("TicksPerCapture")) { + TicksPerCapture = JsonParsed->GetIntegerField("TicksPerCapture"); + } + } else { + UE_LOG(LogHolodeck, Fatal, TEXT("URGBCamera::ParseSensorParms:: Unable to parse json.")); + } +} + +void URGBCamera::InitializeSensor() { + Super::InitializeSensor(); + + //Set up everything for the scenecapturecomponent2d + SceneCapture->CaptureSource = SCS_FinalColorLDR; //Pick what type of output you want to be sent to the texture target. +} + +void URGBCamera::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + + TickCounter++; + if (TickCounter == TicksPerCapture) { + RenderRequest.RetrievePixels(Buffer, TargetTexture); + TickCounter = 0; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RangeFinderSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RangeFinderSensor.cpp new file mode 100644 index 0000000000..deea132508 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RangeFinderSensor.cpp @@ -0,0 +1,75 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "RangeFinderSensor.h" +#include "Json.h" + +URangeFinderSensor::URangeFinderSensor() { + SensorName = "RangeFinderSensor"; +} + +// Allows sensor parameters to be set programmatically from client. +void URangeFinderSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("LaserCount")) { + LaserCount = JsonParsed->GetIntegerField("LaserCount"); + } + + if (JsonParsed->HasTypedField("LaserAngle")) { + LaserAngle = -JsonParsed->GetIntegerField("LaserAngle"); // in client positive angles point up + } + + if (JsonParsed->HasTypedField("LaserMaxDistance")) { + LaserMaxDistance = JsonParsed->GetIntegerField("LaserMaxDistance") * 100; // meters to centimeters + } + + if (JsonParsed->HasTypedField("LaserDebug")) { + LaserDebug = JsonParsed->GetBoolField("LaserDebug"); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("URangeFinderSensor::ParseSensorParms:: Unable to parse json.")); + } +} + +void URangeFinderSensor::InitializeSensor() { + Super::InitializeSensor(); + //You need to get the pointer to the object you are attached to. + Parent = this->GetAttachmentRootActor(); +} + +void URangeFinderSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + + float* FloatBuffer = static_cast(Buffer); + + for (int i = 0; i < LaserCount; i++) { + + FVector start = GetComponentLocation(); + + FVector end = GetForwardVector(); + FVector right = GetRightVector(); + end = end.RotateAngleAxis(-360 * i / LaserCount, GetUpVector()); + right = right.RotateAngleAxis(-360 * i / LaserCount, GetUpVector()); + end = end.RotateAngleAxis(LaserAngle, right); + end = end * LaserMaxDistance; + end = start + end; + + FCollisionQueryParams QueryParams = FCollisionQueryParams(); + QueryParams.AddIgnoredActor(Parent); + + FHitResult Hit = FHitResult(); + + bool TraceResult = GetWorld()->LineTraceSingleByChannel(Hit, start, end, ECollisionChannel::ECC_Visibility, QueryParams); + + FloatBuffer[i] = (TraceResult ? Hit.Distance : LaserMaxDistance) / 100; // centimeter to meters + + if (LaserDebug) { + DrawDebugLine(GetWorld(), start, end, FColor::Green, false, .01, ECC_WorldStatic, 1.f); + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RelativeSkeletalPositionSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RelativeSkeletalPositionSensor.cpp new file mode 100644 index 0000000000..b564791191 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RelativeSkeletalPositionSensor.cpp @@ -0,0 +1,56 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "RelativeSkeletalPositionSensor.h" + +URelativeSkeletalPositionSensor::URelativeSkeletalPositionSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "RelativeSkeletalPositionSensor"; +} + +void URelativeSkeletalPositionSensor::InitializeSensor() { + + AActor* Parent = this->GetOwner(); + + if (Parent->IsA(AAndroid::StaticClass())) { + this->Bones.Append(AAndroid::BoneNames, AAndroid::NumBones); + } + else if (Parent->IsA(AHandAgent::StaticClass())) { + // is AHandAgent + this->Bones.Append(AHandAgent::BoneNames, AHandAgent::NumBones); + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("Error: Tried to use URelativeSkeletalPositionSensor with unknown agent type.")); + } + + TArray Components; + Parent->GetComponents(Components); + this->SkeletalMeshComponent = Components[0]; + + // Save arrays of bones and parent bones + this->ParentBones.Reserve(this->Bones.Num()); + + for (int i = 0; i < this->Bones.Num(); i++) { + this->ParentBones.Insert(this->SkeletalMeshComponent->GetParentBone(this->Bones[i]), i); + } + + // Defer this call to the end because we don't know the size of Bones() at the beginning of the method + Super::InitializeSensor(); +} + +void URelativeSkeletalPositionSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + float* FloatBuffer = static_cast(Buffer); + for (int i = 0; i < Bones.Num(); i++) { + FQuat BoneQ = SkeletalMeshComponent->GetBoneQuaternion(Bones[i], EBoneSpaces::WorldSpace); + FQuat ParentQ = SkeletalMeshComponent->GetBoneQuaternion(ParentBones[i], EBoneSpaces::WorldSpace); + FQuat Quat = ParentQ.Inverse() * BoneQ; + FloatBuffer[4 * i] = Quat.X; + FloatBuffer[4 * i + 1] = Quat.Y; + FloatBuffer[4 * i + 2] = Quat.Z; + FloatBuffer[4 * i + 3] = Quat.W; + } +} + +int URelativeSkeletalPositionSensor::GetNumItems() { + return this->Bones.Num(); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RotationSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RotationSensor.cpp new file mode 100644 index 0000000000..de42049038 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/RotationSensor.cpp @@ -0,0 +1,26 @@ +#include "Holodeck.h" +#include "Conversion.h" +#include "RotationSensor.h" + +URotationSensor::URotationSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "RotationSensor"; +} + +void URotationSensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object the sensor is attached to. + Parent = this->GetAttachmentRootActor(); +} + +void URotationSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (Parent != nullptr && bOn) { + FRotator Rotation = this->GetComponentRotation(); + FVector EulerAngles = RotatorToRPY(Rotation); + float* FloatBuffer = static_cast(Buffer); + FloatBuffer[0] = EulerAngles.X; + FloatBuffer[1] = EulerAngles.Y; + FloatBuffer[2] = EulerAngles.Z; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SidescanSonar.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SidescanSonar.cpp new file mode 100644 index 0000000000..3d621e98b4 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SidescanSonar.cpp @@ -0,0 +1,218 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "Benchmarker.h" +#include "HolodeckBuoyantAgent.h" +#include "SidescanSonar.h" +// #pragma warning (disable : 4101) + +USidescanSonar::USidescanSonar() { + SensorName = "SidescanSonar"; +} + +void USidescanSonar::BeginDestroy() { + Super::BeginDestroy(); + + delete[] count; +} + +// Allows sensor parameters to be set programmatically from client. +void USidescanSonar::ParseSensorParms(FString ParmsJson) { + + // Override the Parent Class defaults for some key parameters + Azimuth = 170; // degrees + Elevation = 0.25; // degrees + RangeMin = 0.5 * 100; // 0.5 m (in cm) + RangeMax = 35 * 100; // 35 m (in cm) + + // Parse any parent class parameters that were provided in the configuration + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + // For handling noise + if (JsonParsed->HasTypedField("AddSigma")) { + addNoise.initSigma(JsonParsed->GetNumberField("AddSigma")); + } + if (JsonParsed->HasTypedField("AddCov")) { + addNoise.initCov(JsonParsed->GetNumberField("AddCov")); + } + if (JsonParsed->HasTypedField("MultSigma")) { + multNoise.initSigma(JsonParsed->GetNumberField("MultSigma")); + } + if (JsonParsed->HasTypedField("MultCov")) { + multNoise.initCov(JsonParsed->GetNumberField("MultCov")); + } + + if (JsonParsed->HasTypedField("RangeRes")) { + RangeRes = JsonParsed->GetNumberField("RangeRes") * 100; // m to cm + } + + if (JsonParsed->HasTypedField("ElevationRes")) { + ElevationRes = JsonParsed->GetNumberField("ElevationRes"); // degrees + } + + if (JsonParsed->HasTypedField("AzimuthRes")) { + AzimuthRes = JsonParsed->GetNumberField("AzimuthRes"); // degrees + } + + if (JsonParsed->HasTypedField("RangeBins")) { + RangeBins = JsonParsed->GetIntegerField("RangeBins"); + } + + if (JsonParsed->HasTypedField("AzimuthBins")) { + AzimuthBins = JsonParsed->GetIntegerField("AzimuthBins"); + } + + if (JsonParsed->HasTypedField("ElevationBins")) { + ElevationBins = JsonParsed->GetIntegerField("ElevationBins"); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("USidescanSonar::ParseSensorParms:: Unable to parse json.")); + } + + // Parse through the Range parameters given to us + if(RangeBins != 0){ + RangeRes = (RangeMax - RangeMin) / RangeBins; + } + else if(RangeRes != 0){ + RangeBins = (RangeMax - RangeMin) / RangeRes; + } + else{ + RangeRes = 5; // cm + RangeBins = (RangeMax - RangeMin) / RangeRes; + } + + // Parse through the Azimuth parameters given to us + if(AzimuthBins != 0){ + AzimuthRes = Azimuth / AzimuthBins; + } + else if(AzimuthRes != 0){ + AzimuthBins = Azimuth / AzimuthRes; + } + else{ + // Calculate how large the azimuth bins should be + AzimuthRes = (180 * Octree::OctreeMin) / (Pi * (RangeMin + 0.1 * (RangeMax - RangeMin))); + AzimuthBins = Azimuth / AzimuthRes; + } + + // Parse through the Elevation parameters given to us + if(ElevationBins != 0){ + ElevationRes = Elevation / ElevationBins; + } + else if(ElevationRes != 0){ + ElevationBins = Elevation / ElevationRes; + } + else{ + // Calculate how large our shadowing bins should be + ElevationBins = (RangeMin*Elevation*Pi/180) / Octree::OctreeMin; + if(ElevationBins < 1) ElevationBins = 1; + ElevationRes = Elevation / ElevationBins; + } +} + +void USidescanSonar::InitializeSensor() { + Super::InitializeSensor(); + + // setup count of each bin + count = new int32[RangeBins](); // Sidescan Sonar (1d array) + + for(int i=0;i()); + sortedLeaves[i].Reserve(10000); + } +} + +// Conversion from Spherical coordinates to Euclidian +FVector spherToEucSS(float r, float theta, float phi, FTransform SensortoWorld){ + float x = r*UKismetMathLibrary::DegSin(phi)*UKismetMathLibrary::DegCos(theta); + float y = r*UKismetMathLibrary::DegSin(phi)*UKismetMathLibrary::DegSin(theta); + float z = r*UKismetMathLibrary::DegCos(phi); + return UKismetMathLibrary::TransformLocation(SensortoWorld, FVector(x, y, z)); +} + + +void USidescanSonar::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + Super::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); + + if(TickCounter == 0){ + // reset things and get ready + float* result = static_cast(Buffer); + std::fill(result, result+RangeBins, 0); + std::fill(count, count+RangeBins, 0); + + for(auto& sl: sortedLeaves){ + sl.Reset(); + } + + // Finds leaves in range and puts them in foundLeaves + findLeaves(); + + + // SORT THEM INTO AZIMUTH/ELEVATION BINS + int32 idx; + for(TArray& bin : foundLeaves){ + for(Octree* l : bin){ + // Compute bins while we're parallelized + l->idx.Y = (int32)((l->locSpherical.Y - minAzimuth)/ AzimuthRes); + l->idx.Z = (int32)((l->locSpherical.Z - minElev)/ ElevationRes); + // Sometimes we get float->int rounding errors + if(l->idx.Y == AzimuthBins) --l->idx.Y; + + // UE_LOG(LogTemp, Warning, TEXT("Index Y: %d"), l->idx.Y); + + idx = l->idx.Z*AzimuthBins + l->idx.Y; + sortedLeaves[idx].Emplace(l); + } + } + + + // HANDLE SHADOWING + shadowLeaves(); + + + // ADD IN ALL CONTRIBUTIONS + // Reuse idx variable from above + for(TArray& bin : sortedLeaves){ + for(Octree* l : bin){ + // Calculate range bin + l->idx.X = (int32)((l->locSpherical.X - RangeMin) / RangeRes); + + // Add to their appropriate bin + if (l->idx.Y > (AzimuthBins / 2)){ + idx = RangeBins / 2 - l->idx.X / 2 - 1; + } + else{ + idx = RangeBins / 2 + l->idx.X / 2; + } + + result[idx] += l->val; + ++count[idx]; + } + } + + + // NORMALIZE THE BUFFER + for (int i = 0; i < RangeBins; i++) { + if(count[i] != 0){ + result[i] *= (1 + multNoise.sampleFloat()) / count[i]; + result[i] += addNoise.sampleRayleigh(); + } + else{ + result[i] = addNoise.sampleRayleigh(); + } + } + } + + if (runtickCounter == 20 && (RangeMin*Elevation*Pi/180) / Octree::OctreeMin < 1) + { + float recommendedElevation = Octree::OctreeMin * 180 / (RangeMin * Pi); + float recommendedOctreeMin = RangeMin * Elevation * Pi / 180 / 100; + GEngine->AddOnScreenDebugMessage(-1, 5.0f, FColor::Red, FString::Printf(TEXT("WARNING: Elevation angle potentially too small with current OctreeMin configuration\n Recommended changes (pick one):\n Elevation = %f\n OctreeMin = %f\n"), recommendedElevation, recommendedOctreeMin)); + } + + runtickCounter++; +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SinglebeamSonar.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SinglebeamSonar.cpp new file mode 100644 index 0000000000..be5b0cca5b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/SinglebeamSonar.cpp @@ -0,0 +1,264 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#include "Holodeck.h" +#include "Benchmarker.h" +#include "HolodeckBuoyantAgent.h" +#include "SinglebeamSonar.h" +// #pragma warning (disable : 4101) + +USinglebeamSonar::USinglebeamSonar() { + SensorName = "SinglebeamSonar"; +} + +void USinglebeamSonar::BeginDestroy() { + Super::BeginDestroy(); + + delete[] count; +} + +// Allows sensor parameters to be set programmatically from client. +void USinglebeamSonar::ParseSensorParms(FString ParmsJson) { + + // range in cm + RangeMin = 0.5 * 100; + RangeMax = 10 * 100; + + Super::ParseSensorParms(ParmsJson); + + // user can override default values + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + // Geometry Parameters + if (JsonParsed->HasTypedField("OpeningAngle")) { + OpeningAngle = JsonParsed->GetNumberField("OpeningAngle"); + } + if (JsonParsed->HasTypedField("RangeBins")) { + RangeBins = JsonParsed->GetIntegerField("RangeBins"); + } + if (JsonParsed->HasTypedField("RangeRes")) { + RangeRes = JsonParsed->GetNumberField("RangeRes")*100; + } + + // Advanced Params + if (JsonParsed->HasTypedField("OpeningAngleBins")) { + OpeningAngleBins = JsonParsed->GetIntegerField("OpeningAngleBins"); + } + if (JsonParsed->HasTypedField("OpeningAngleRes")) { + OpeningAngleRes = JsonParsed->GetNumberField("OpeningAngleRes"); + } + if (JsonParsed->HasTypedField("CentralAngleBins")) { + CentralAngleBins = JsonParsed->GetIntegerField("CentralAngleBins"); + } + if (JsonParsed->HasTypedField("CentralAngleRes")) { + CentralAngleRes = JsonParsed->GetNumberField("CentralAngleRes"); + } + + // If the user hasn't set this, this is a better default value + // If they have set it, the super class will take care of it + if (!JsonParsed->HasTypedField("ShadowEpsilon")) { + ShadowEpsilon = Octree::OctreeMin; + } + + // Noise Parameters + if (JsonParsed->HasTypedField("AddSigma")) { + addNoise.initSigma(JsonParsed->GetNumberField("AddSigma")); + } + if (JsonParsed->HasTypedField("AddCov")) { + addNoise.initCov(JsonParsed->GetNumberField("AddCov")); + } + if (JsonParsed->HasTypedField("MultSigma")) { + multNoise.initSigma(JsonParsed->GetNumberField("MultSigma")); + } + if (JsonParsed->HasTypedField("MultCov")) { + multNoise.initCov(JsonParsed->GetNumberField("MultCov")); + } + if (JsonParsed->HasTypedField("RangeSigma")) { + rNoise.initBounds(JsonParsed->GetNumberField("RangeSigma")*100); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("USinglebeamSonar::ParseSensorParms:: Unable to parse json.")); + } + + // Parse through the Range parameters given to us + if(RangeBins != 0){ + RangeRes = (RangeMax - RangeMin) / RangeBins; + } + else if(RangeRes != 0){ + RangeBins = (RangeMax - RangeMin) / RangeRes; + } + else{ + RangeBins = 200; + RangeRes = (RangeMax - RangeMin) / RangeBins; + } + + // Parse through the OpeningAngle parameters given to us + if(OpeningAngleBins != 0){ + OpeningAngleRes = OpeningAngle / OpeningAngleBins; + } + else if(OpeningAngleRes != 0){ + OpeningAngleBins = OpeningAngle / OpeningAngleRes; + } + else{ + // Calculate how large our shadowing bins should be + float dist = RangeMin; + OpeningAngleBins = (dist*OpeningAngle*Pi/180) / Octree::OctreeMin; + if(OpeningAngleBins < 1) OpeningAngleBins = 1; + OpeningAngleRes = OpeningAngle / OpeningAngleBins; + } + + // Parse through the CentralAngle parameters given to us + if(CentralAngleBins != 0){ + CentralAngleRes = CentralAngle / CentralAngleBins; + } + else if(CentralAngleRes != 0){ + CentralAngleBins = CentralAngle / CentralAngleRes; + } + else{ + // Calculate how large our shadowing bins should be + float dist = RangeMin; + CentralAngleBins = (dist*CentralAngle*Pi/180) / Octree::OctreeMin; + if(CentralAngleBins < 6) CentralAngleBins = 6; + CentralAngleRes = CentralAngle / CentralAngleBins; + } +} + +void USinglebeamSonar::InitializeSensor() { + Super::InitializeSensor(); + + // Setup bins + CentralAngle = 360; + + minCentralAngle = -180; + maxCentralAngle = 180; + minOpeningAngle = 0; + maxOpeningAngle = OpeningAngle/2; + + // setup count of each bin + count = new int32[RangeBins](); + + for(int i=0;i()); + sortedLeaves[i].Reserve(10000); + } + + // Cache some calculations for later + sqrt3_2 = UKismetMathLibrary::Sqrt(3)/2; + sinOffset = UKismetMathLibrary::DegSin(FGenericPlatformMath::Min(CentralAngle, OpeningAngle)/2); +} + + +// determine if a single leaf is in your tree +bool USinglebeamSonar::inRange(Octree* tree){ + FTransform SensortoWorld = this->GetComponentTransform(); + // if it's not a leaf, we use a bigger search area + float offset = 0; + float radius = 0; + + if(tree->size != Octree::OctreeMin){ + radius = tree->size*sqrt3_2; + offset = radius/sinOffset; + SensortoWorld.AddToTranslation( -this->GetForwardVector()*offset ); + } + + // transform location to sensor frame instead of global (x y z) + FVector locLocal = SensortoWorld.GetRotation().UnrotateVector(tree->loc-SensortoWorld.GetTranslation()); + + // check if it's in range + tree->locSpherical.X = locLocal.Size(); + if(RangeMin+offset-radius >= tree->locSpherical.X || tree->locSpherical.X >= RangeMax+offset+radius) return false; + + // check if OpeningAngle is in range. OpeningAngle is angle off of x-axis + tree->locSpherical.Z = ATan2Approx(UKismetMathLibrary::Sqrt(UKismetMathLibrary::Square(locLocal.Y)+UKismetMathLibrary::Square(locLocal.Z)), locLocal.X); //OpeningAngle of leaf we are inspecting + if(minOpeningAngle >= tree->locSpherical.Z || tree->locSpherical.Z >= maxOpeningAngle) return false; + + // save CentralAngle for shadowing later. CentralAngle goes around the x-axis + tree->locSpherical.Y = ATan2Approx(locLocal.Z, locLocal.Y); + + // otherwise it's in! + return true; +} + + +void USinglebeamSonar::showRegion(float DeltaTime){ + if(ViewRegion){ + float debugThickness = 3.0f; + float DebugNumSides = 6; //change later? + float length = (RangeMax - RangeMin); //length of cone in cm + + DrawDebugCone(GetWorld(), GetComponentLocation(), GetForwardVector(), length, (OpeningAngle/2)*Pi/180, (OpeningAngle/2)*Pi/180, DebugNumSides, FColor::Green, false, .00, ECC_WorldStatic, debugThickness); + } +} + +void USinglebeamSonar::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + Super::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); + + if(TickCounter == 0){ + // reset things and get ready + float* result = static_cast(Buffer); + std::fill(result, result+RangeBins, 0); + std::fill(count, count+RangeBins, 0); + + for(auto& sl: sortedLeaves){ + sl.Reset(); + } + + // Finds leaves in range and puts them in foundLeaves + findLeaves(); // does not return anything, saves to foundLeaves + + + // SORT THEM INTO CENTRALANGLE/OPENINGANGLE BINS + int32 idx; + for(TArray& bin : foundLeaves){ + for(Octree* l : bin){ + // Compute bins while we're parallelized + l->idx.Y = (int32)((l->locSpherical.Y - minCentralAngle)/ CentralAngleRes); + l->idx.Z = (int32)((l->locSpherical.Z - minOpeningAngle)/ OpeningAngleRes); + // Sometimes we get float->int rounding errors + if(l->idx.Y == CentralAngleBins) --l->idx.Y; + + idx = l->idx.Z*CentralAngleBins + l->idx.Y; + // array of arrays (the rectangle we split off) + sortedLeaves[idx].Emplace(l); + } + } + + // HANDLE SHADOWING + shadowLeaves(); + + + // ADD IN ALL CONTRIBUTIONS + float range_noise; + for(TArray& bin : sortedLeaves){ + for(Octree* l : bin){ + // Add noise to each of them + range_noise = rNoise.sampleExponential(); + l->idx.X = (int32)((l->locSpherical.X - RangeMin + range_noise) / RangeRes); + + // In case our noise has pushed us out of range + if(l->idx.X >= RangeBins) l->idx.X = RangeBins-1; + + // Add to their appropriate bin + idx = l->idx.X; + + result[idx] += l->val; + ++count[idx]; + } + } + + + // MOVE THEM INTO BUFFER + for (int i = 0; i < RangeBins; i++) { + if(count[i] != 0){ + + // actually take the average of the intensities + result[i] *= (1 + multNoise.sampleFloat())/count[i]; + result[i] += addNoise.sampleRayleigh(); + } + else{ + result[i] = addNoise.sampleRayleigh(); + } + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/VelocitySensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/VelocitySensor.cpp new file mode 100644 index 0000000000..c025bddc32 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/VelocitySensor.cpp @@ -0,0 +1,26 @@ +#include "Holodeck.h" +#include "VelocitySensor.h" + +UVelocitySensor::UVelocitySensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "VelocitySensor"; +} + +void UVelocitySensor::InitializeSensor() { + Super::InitializeSensor(); + + //You need to get the pointer to the object the sensor is attached to. + Parent = Cast(this->GetAttachParent()); +} + +void UVelocitySensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //check if your parent pointer is valid, and if the sensor is on. Then get the velocity and buffer, then send the data to it. + if (Parent != nullptr && bOn) { + FVector Velocity = Parent->GetPhysicsLinearVelocityAtPoint(this->GetComponentLocation()); + Velocity = ConvertLinearVector(Velocity, UEToClient); + float* FloatBuffer = static_cast(Buffer); + FloatBuffer[0] = Velocity.X; + FloatBuffer[1] = Velocity.Y; + FloatBuffer[2] = Velocity.Z; + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ViewportCapture.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ViewportCapture.cpp new file mode 100644 index 0000000000..5d7e773ae3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/ViewportCapture.cpp @@ -0,0 +1,61 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "ViewportCapture.h" +#include "Json.h" + +// Sets default values for this component's properties +UViewportCapture::UViewportCapture(){ + PrimaryComponentTick.bCanEverTick = true; + SensorName = "ViewportCapture"; +} + +// Allows sensor parameters to be set programmatically from client. +void UViewportCapture::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("bGrayScale")) { + bGrayScale = JsonParsed->GetBoolField("bGrayScale"); + } + + if (JsonParsed->HasTypedField("Width")) { + Width = JsonParsed->GetIntegerField("Width"); + } + + if (JsonParsed->HasTypedField("Height")) { + Height = JsonParsed->GetIntegerField("Height"); + } + } else { + UE_LOG(LogHolodeck, Fatal, TEXT("URGBCamera::ParseSensorParms:: Unable to parse json.")); + } +} + +void UViewportCapture::InitializeSensor() { + UE_LOG(LogHolodeck, Log, TEXT("UViewportCapture::InitializeSensor")); + + // This must come first, since the HolodeckSensor parent class will + // call GetNumItems, which needs the ViewportClient. + ViewportClient = Cast(GEngine->GameViewport); + if (ViewportClient && bOn) { + const FVector2D ViewportSize = FVector2D(ViewportClient->Viewport->GetSizeXY()); + Width = ViewportSize.X; + Height = ViewportSize.Y; + } + + // Buffers must be set after width and height are retrieved. + Super::InitializeSensor(); + ViewportClient->SetBuffer(Buffer); +} + +void UViewportCapture::TickSensorComponent(float DeltaTime, + ELevelTick TickType, + FActorComponentTickFunction* ThisTickFunction) { + // The pixel data is captured on the rendering thread. + // All this class needs to do is set the buffer at begin play. + if (!ViewportClient->BufferIsSet()) + ViewportClient->SetBuffer(Buffer); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/WorldNumSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/WorldNumSensor.cpp new file mode 100644 index 0000000000..ce9a1c4898 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Private/WorldNumSensor.cpp @@ -0,0 +1,40 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "WorldNumSensor.h" +#include "HolodeckGameMode.h" + +UWorldNumSensor::UWorldNumSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "WorldNumSensor"; +} + +void UWorldNumSensor::InitializeSensor() { + Super::InitializeSensor(); +} + +void UWorldNumSensor::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Key")) { + Key = JsonParsed->GetStringField("Key"); + } + } + else { + UE_LOG(LogHolodeck, Warning, TEXT("%s Unable to parse json."), *FString(__func__)); + } +} + +void UWorldNumSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + //Check if the sensor is on and if it is retrieve value from the world state + if (bOn && Key != "") { + AActor* Target = GetWorld()->GetAuthGameMode(); + AHolodeckGameMode* Game = static_cast(Target); + int* IntBuffer = static_cast(Buffer); + IntBuffer[0] = (int32)Game->GetWorldNum(Key); + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AbuseSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AbuseSensor.h new file mode 100644 index 0000000000..1829d7c41e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AbuseSensor.h @@ -0,0 +1,43 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSensor.h" +#include "AbuseSensor.generated.h" + +/** + * AbuseSensor + * Inherits from the HolodeckSensor class + * Check out the parent class for documentation on all of the overridden functions. + * Returns true if the agent has been abused. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UAbuseSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UAbuseSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 1; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + AHolodeckAgent* Agent; + FVector PrevSpeed; + float AccelerationLimit; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AcousticBeaconSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AcousticBeaconSensor.h new file mode 100644 index 0000000000..f757b593a8 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/AcousticBeaconSensor.h @@ -0,0 +1,51 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" +#include "HolodeckSensor.h" + +#include +#include "Kismet/KismetMathLibrary.h" + +#include "AcousticBeaconSensor.generated.h" + +/** + * AcousticBeaconSensor + * Inherits from the HolodeckSensor class + * Check out the parent class for documentation on all of the overridden funcions. + * Gets the true velocity of the component that the sensor is attached to. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UAcousticBeaconSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UAcousticBeaconSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + UAcousticBeaconSensor* fromSensor = NULL; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 4; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /** + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + * Not owned. + */ + UPrimitiveComponent* Parent; + float WaitBuffer[4]; + int WaitTicks = -1; + float SpeedOfSound = 1500; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DVLSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DVLSensor.h new file mode 100644 index 0000000000..70c785d7a2 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DVLSensor.h @@ -0,0 +1,75 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "MultivariateNormal.h" +#include "Kismet/KismetMathLibrary.h" + +#include "DVLSensor.generated.h" + +/** + * DVLSensor + * Inherits from the HolodeckSensor class + * Check out the parent class for documentation on all of the overridden funcions. + * Gets the true velocity of the component that the sensor is attached to. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UDVLSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UDVLSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return ReturnRange ? 7 : 3; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + bool DebugLines = false; + + UPROPERTY(EditAnywhere) + float elevation = 90; + + UPROPERTY(EditAnywhere) + bool ReturnRange = true; + + UPROPERTY(EditAnywhere) + float MaxRange = 20*100; + +private: + /** + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + * Not owned. + */ + UPrimitiveComponent* Parent; + + // Used for noise + float sinElev; + float cosElev; + MultivariateNormal<4> mvnVel; + MultivariateNormal<4> mvnRange; + TArray> transform; + + // used for debugging + TArray directions; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DepthSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DepthSensor.h new file mode 100644 index 0000000000..8dfc2898a9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/DepthSensor.h @@ -0,0 +1,52 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "MultivariateNormal.h" + +#include "DepthSensor.generated.h" + +/** +* DepthSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden functions. +* Reports the XYZ coordinate of the parent agent. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UDepthSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UDepthSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 1; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + USceneComponent* Parent; + MultivariateNormal<1> mvn; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/GPSSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/GPSSensor.h new file mode 100644 index 0000000000..2fd719cdaf --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/GPSSensor.h @@ -0,0 +1,57 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include +#include "MultivariateNormal.h" + +#include "GPSSensor.generated.h" + +/** +* GPSSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden functions. +* Reports the XYZ coordinate of the parent agent. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UGPSSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UGPSSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 3; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + float GPSDepth = 2; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + USceneComponent* Parent; + MultivariateNormal<3> mvn; + MultivariateNormal<1> depthMVN; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCamera.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCamera.h new file mode 100644 index 0000000000..1470fea27d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCamera.h @@ -0,0 +1,60 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" +#include "HolodeckViewportClient.h" +#include "RenderRequest.h" +#include "HolodeckCamera.generated.h" + +/** +* HolodeckCamera +* Abstract base class for cameras within holodeck +* A camera is anything that needs to access visual information. +* Two examples include a depth sensor and a standard camera. +*/ +UCLASS(Blueprintable, ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UHolodeckCamera : public UHolodeckSensor +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UHolodeckCamera(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction); + FColor* Buffer; + FRenderRequest RenderRequest; + + UPROPERTY() + UTextureRenderTarget2D* TargetTexture; + + UPROPERTY() + USceneCaptureComponent2D* SceneCapture; + + UPROPERTY(EditAnywhere) + int CaptureWidth = 256; + + UPROPERTY(EditAnywhere) + int CaptureHeight = 256; + +private: + + bool bPointerGivenToViewport = false; + UHolodeckViewportClient* ViewportClient; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCollisionSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCollisionSensor.h new file mode 100644 index 0000000000..367f309f85 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/HolodeckCollisionSensor.h @@ -0,0 +1,53 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "Components/SceneComponent.h" +#include "HolodeckSensor.h" + +#include "HolodeckCollisionSensor.generated.h" + +/** +* UHolodeckCollisionSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden funcions. +* Reports whether the parent agent is currenty colliding with any other object. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UHolodeckCollisionSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UHolodeckCollisionSensor(); + + /** + * InitializeComponent + * Called on level load, before begin play + * It's important for these things to be initialized before the its actor's BeginPlay() is called. + */ + void InitializeComponent() override; + + UFUNCTION() + void OnHit(AActor* SelfActor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit); + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 1; }; + int GetItemSize() override { return sizeof(bool); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /** + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + * Not owned. + */ + AActor* Parent; + + bool bIsColliding = false; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/IMUSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/IMUSensor.h new file mode 100644 index 0000000000..ddb4b4d6c9 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/IMUSensor.h @@ -0,0 +1,94 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "MultivariateNormal.h" + +#include "IMUSensor.generated.h" + +/** + * An intertial measurement unit. + * Returns a 1D numpy array of: + * `[acceleration_x, acceleration_y, acceleration_z, velocity_roll, velocity_pitch, velocity_yaw]` + */ +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) +class HOLODECK_API UIMUSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UIMUSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * GetAccelerationVector + * Gets the acceleration vector. + * @return the acceleration vector. + */ + FVector GetAccelerationVector(); + + /** + * GetAngularVelocityVector + * Gets the angular velocity vector. + * @return the angular velocity vector. + */ + FVector GetAngularVelocityVector(); + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + // See HolodeckSensor for more information on these overridden functions. + int GetNumItems() override { return ReturnBias ? 12 : 6; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + bool ReturnBias = false; + +private: + /** + * CalculateAccelerationVector + * Calculates the acceleration vector. + * @param DeltaTime the time that has passsed since the last tick. + */ + void CalculateAccelerationVector(float DeltaTime); + + /** + * CalculateAngularVelocityVector + * Calculates the angular velocity vector. + */ + void CalculateAngularVelocityVector(); + + UPrimitiveComponent* Parent; + + UWorld* World; + AWorldSettings* WorldSettings; + float WorldGravity; + + FVector VelocityThen; + FVector VelocityNow; + FRotator RotationNow; + + FVector LinearAccelerationVector; + FVector AngularVelocityVector; + + // Used for noise + MultivariateNormal<3> mvnAccel; + MultivariateNormal<3> mvnOmega; + MultivariateNormal<3> mvnBiasAccel; + MultivariateNormal<3> mvnBiasOmega; + FVector BiasAccel = FVector(0); + FVector BiasOmega = FVector(0); +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ImagingSonar.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ImagingSonar.h new file mode 100644 index 0000000000..43adabf0d1 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ImagingSonar.h @@ -0,0 +1,109 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSonar.h" + +#include "GenericPlatform/GenericPlatformMath.h" +#include "Octree.h" +#include "Kismet/KismetMathLibrary.h" +#include "Async/ParallelFor.h" +#include "MultivariateNormal.h" +#include "MultivariateUniform.h" + +#include +#include "Json.h" + +#include "ImagingSonar.generated.h" + +#define Pi 3.1415926535897932384626433832795 +/** + * UImagingSonar + */ +UCLASS() +class HOLODECK_API UImagingSonar : public UHolodeckSonar +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UImagingSonar(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + /* + * Cleans up octree + */ + virtual void BeginDestroy() override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return RangeBins*AzimuthBins; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + int32 RangeBins = 0; + + UPROPERTY(EditAnywhere) + float RangeRes = 0; + + UPROPERTY(EditAnywhere) + int32 AzimuthBins = 0; + + UPROPERTY(EditAnywhere) + float AzimuthRes = 0; + + UPROPERTY(EditAnywhere) + int32 ElevationBins = 0; + + UPROPERTY(EditAnywhere) + float ElevationRes = 0; + + UPROPERTY(EditAnywhere) + bool MultiPath = false; + + UPROPERTY(EditAnywhere) + int32 ClusterSize = 5; + + UPROPERTY(EditAnywhere) + bool ScaleNoise = true; + + UPROPERTY(EditAnywhere) + int32 AzimuthStreaks = 0; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; + + // various computations we want to cache + int32 AzimuthBinScale = 1; + float perfectCos; + + // Used to hold leaves for multipath + TMap mapLeaves; + TMap mapSearch; + TArray> cluster; + int32* count; + int32* hasPerfectNormal; + + // for adding noise + MultivariateNormal<1> addNoise; + MultivariateNormal<1> multNoise; + MultivariateUniform<1> rNoise; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/JointRotationSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/JointRotationSensor.h new file mode 100644 index 0000000000..abbc44eb9f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/JointRotationSensor.h @@ -0,0 +1,53 @@ +#pragma once + +#include "Holodeck.h" +#include "Android.h" +#include "HandAgent.h" + +#include "HolodeckPawnController.h" +#include "HolodeckSensor.h" +#include "PhysicsEngine/ConstraintInstance.h" + +#include "JointRotationSensor.generated.h" + +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UJointRotationSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UJointRotationSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + // See HolodeckSensor for information on these classes. + virtual int GetNumItems() override; + virtual int GetItemSize() override { return sizeof(float); }; + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + TArray Joints; + TArray ParentBones; + USkeletalMeshComponent* SkeletalMeshComponent; + AActor* Parent; + + int TotalDof, Num3AxisJoints, Num2AxisJoints; + /** + * AddJointRotationToBuffer + * Adds a certain joint rotation to the buffer. + * @param JointName the name of the joint to add. + * @param Swing1 true to insert the swing1 value. + * @param Swing2 true to insert the swing2 value. + * @param Twist true to insert the twist value. + * @param Data a pointer into the data buffer at the point the data should be inserted + * @return a pointer to the next position in the buffer. + */ + float* AddJointRotationToBuffer(FString JointName, bool Swing1, bool Swing2, bool Twist, float* Data); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/LocationSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/LocationSensor.h new file mode 100644 index 0000000000..b29096a345 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/LocationSensor.h @@ -0,0 +1,52 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "MultivariateNormal.h" + +#include "LocationSensor.generated.h" + +/** +* LocationSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden functions. +* Reports the XYZ coordinate of the parent agent. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API ULocationSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + ULocationSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 3; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + USceneComponent* Parent; + MultivariateNormal<3> mvn; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OpticalModemSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OpticalModemSensor.h new file mode 100644 index 0000000000..a0c2321809 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OpticalModemSensor.h @@ -0,0 +1,61 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file +#pragma once +#include "Holodeck.h" +#include "HolodeckSensor.h" +#include "Kismet/KismetMathLibrary.h" +#include "MultivariateNormal.h" + +#include "OpticalModemSensor.generated.h" + + +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UOpticalModemSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + UOpticalModemSensor(); + + virtual void InitializeSensor() override; + UOpticalModemSensor* FromSensor = nullptr; + +virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + int GetNumItems() override { return 1; }; + + int GetItemSize() override { return sizeof(bool); } + + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + //Max distance of the modem in meters + UPROPERTY(EditAnywhere) + float MaxDistance = 50; + + float NoiseMaxDistance; + + //Debug Variables + UPROPERTY(EditAnywhere) + float LaserAngle = 60; + + float NoiseLaserAngle; + + UPROPERTY(EditAnywhere) + bool LaserDebug = false; + + UPROPERTY(EditAnywhere) + int DebugNumSides = 72; //Default so that each side is 5 degrees + + UPROPERTY(EditAnywhere) + FColor DebugColor = FColor::Green; + + +private: + + AActor* Parent; + bool IsSensorOriented(UOpticalModemSensor* Sensor, FVector LocalToSensor); + bool CanTransmit(); + TMap ColorMap; + void FillColorMap(); + MultivariateNormal<1> DistanceNoise; + MultivariateNormal<1> AngleNoise; +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OrientationSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OrientationSensor.h new file mode 100644 index 0000000000..c9bae6165d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/OrientationSensor.h @@ -0,0 +1,45 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "HolodeckSensor.h" + +#include "OrientationSensor.generated.h" + +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) + +/** +* UOrientationSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden funcions. +* Gives the complete orientation of the parent agent in three vectors: forward, right, and up. +*/ +class HOLODECK_API UOrientationSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UOrientationSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + // See HolodeckSensor for documentation on these classes. + int GetNumItems() override { return 9; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + UPrimitiveComponent* Parent; + UStaticMeshComponent* RootMesh; + UWorld* World; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PoseSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PoseSensor.h new file mode 100644 index 0000000000..a794580299 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PoseSensor.h @@ -0,0 +1,45 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckPawnController.h" +#include "HolodeckSensor.h" + +#include "PoseSensor.generated.h" + +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) + +/** +* UPoseSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden funcions. +* Gives the complete orientation of the parent agent in three vectors: forward, right, and up. +*/ +class HOLODECK_API UPoseSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UPoseSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + // See HolodeckSensor for documentation on these classes. + int GetNumItems() override { return 16; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + UPrimitiveComponent* Parent; + UStaticMeshComponent* RootMesh; + UWorld* World; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PressureSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PressureSensor.h new file mode 100644 index 0000000000..7d4828e3ce --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/PressureSensor.h @@ -0,0 +1,62 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "HolodeckSensor.h" +#include "Android.h" +#include "HandAgent.h" + +#include "Components/SceneComponent.h" + +#include "PressureSensor.generated.h" + +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) +/** +* UPressureSensor +*/ +class HOLODECK_API UPressureSensor : public UHolodeckSensor +{ + GENERATED_BODY() + +public: + // Sets default values for this component's properties + UPressureSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Callback function that is called whenever part of the actor is hit. + */ + UFUNCTION() + void OnHit(AActor* SelfActor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit); + + virtual void BeginDestroy() override; + +private: + + float *PrivateData; + int NumJoints; + FName *Joints; + +protected: + // 94 DOF each with 2 length 3 vectors containing impulse normal info and hit location + int GetNumItems() override; + int GetItemSize() override { return sizeof(float); }; + + // Called every frame + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + float* AddHitToBuffer(FString BoneName, FVector HitBoneLocation, FVector NormalImpulse, float* Data); + + AActor* Parent; + + + USkeletalMeshComponent* SkeletalMeshComponent; + + void InitJointMap(); + TMap JointMap; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ProfilingSonar.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ProfilingSonar.h new file mode 100644 index 0000000000..70a6fd2b04 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ProfilingSonar.h @@ -0,0 +1,31 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "Holodeck.h" +#include "HolodeckSensor.h" + +#include "ImagingSonar.h" + +#include "ProfilingSonar.generated.h" + +/** + * UProfilingSonar + */ +UCLASS() +class HOLODECK_API UProfilingSonar : public UImagingSonar +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UProfilingSonar(); + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RGBCamera.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RGBCamera.h new file mode 100644 index 0000000000..aa9b78eeba --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RGBCamera.h @@ -0,0 +1,39 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckCamera.h" +#include "RGBCamera.generated.h" + +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API URGBCamera : public UHolodeckCamera { + GENERATED_BODY() + +public: + URGBCamera(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + UPROPERTY(EditAnywhere) + int TicksPerCapture = 1; + +protected: + //Checkout HolodeckSensor.h for the documentation on these overridden functions. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction); + + virtual int GetNumItems() { return CaptureWidth * CaptureHeight; }; + virtual int GetItemSize() { return sizeof(float); }; + +private: + int TickCounter = 0; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RangeFinderSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RangeFinderSensor.h new file mode 100644 index 0000000000..d1e823e8be --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RangeFinderSensor.h @@ -0,0 +1,64 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSensor.h" +#include "RangeFinderSensor.generated.h" + +/** + * URangeFinderSensor + * The RangeFinderSensor gets distances to first collision. + * By default the sensor returns one number that represents the distance to + * the first collision in the agent's forward direction. Increasing LaserCount + * will create an array of sensors evenly distributed 360 degrees in a plane + * around the agent. The LaserAngle offsets each of these and transforms the + * plane into a cone. The default MaxDistance is 1000 cm. + */ +UCLASS() +class HOLODECK_API URangeFinderSensor : public UHolodeckSensor +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + URangeFinderSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return LaserCount; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + int LaserCount = 1; + + UPROPERTY(EditAnywhere) + int LaserAngle = 0; + + UPROPERTY(EditAnywhere) + int LaserMaxDistance = 1000; + + UPROPERTY(EditAnywhere) + bool LaserDebug = false; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RelativeSkeletalPositionSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RelativeSkeletalPositionSensor.h new file mode 100644 index 0000000000..e04d215595 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RelativeSkeletalPositionSensor.h @@ -0,0 +1,39 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" +#include "Android.h" +#include "HandAgent.h" + +#include "Components/SceneComponent.h" +#include "HolodeckSensor.h" + +#include "RelativeSkeletalPositionSensor.generated.h" + +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) +class HOLODECK_API URelativeSkeletalPositionSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + URelativeSkeletalPositionSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + int GetNumItems() override; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + USkeletalMeshComponent* SkeletalMeshComponent; + TArray Bones; + TArray ParentBones; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RotationSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RotationSensor.h new file mode 100644 index 0000000000..263563b783 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/RotationSensor.h @@ -0,0 +1,45 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "RotationSensor.generated.h" + +/** +* RotationSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden funcions. +* Gets the true rotation of the component that the sensor is attached to. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API URotationSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + URotationSensor (); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 3; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /** + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; + +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SidescanSonar.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SidescanSonar.h new file mode 100644 index 0000000000..1d6510baa0 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SidescanSonar.h @@ -0,0 +1,87 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSonar.h" + +#include "GenericPlatform/GenericPlatformMath.h" +#include "Octree.h" +#include "Kismet/KismetMathLibrary.h" +#include "Async/ParallelFor.h" +#include "MultivariateNormal.h" + +#include "Json.h" + +#include "SidescanSonar.generated.h" + +#define Pi 3.1415926535897932384626433832795 +/** + * USidescanSonar + */ +UCLASS() +class HOLODECK_API USidescanSonar : public UHolodeckSonar +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + USidescanSonar(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + /* + * Cleans up octree + */ + virtual void BeginDestroy() override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return RangeBins; }; // Returns 1D array for buffer for Sidescan Sonar + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + int32 RangeBins = 0; + + UPROPERTY(EditAnywhere) + float RangeRes = 0; + + UPROPERTY(EditAnywhere) + int32 AzimuthBins = 0; + + UPROPERTY(EditAnywhere) + float AzimuthRes = 0; + + UPROPERTY(EditAnywhere) + int32 ElevationBins = 0; + + UPROPERTY(EditAnywhere) + float ElevationRes = 0; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; + + // Used for counting how many leaves in a bin for averaging at the end + int32* count; + uint32 runtickCounter = 0; + + // for adding noise + MultivariateNormal<1> addNoise; + MultivariateNormal<1> multNoise; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SinglebeamSonar.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SinglebeamSonar.h new file mode 100644 index 0000000000..4698e92104 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/SinglebeamSonar.h @@ -0,0 +1,107 @@ +// MIT License (c) 2021 BYU FRoStLab see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSonar.h" + +#include "GenericPlatform/GenericPlatformMath.h" +#include "Octree.h" +#include "Kismet/KismetMathLibrary.h" +#include "Async/ParallelFor.h" +#include "MultivariateNormal.h" +#include "MultivariateUniform.h" + +#include "Json.h" + +#include "SinglebeamSonar.generated.h" + +#define Pi 3.1415926535897932384626433832795 +/** + * USinglebeamSonar + */ +UCLASS() +class HOLODECK_API USinglebeamSonar : public UHolodeckSonar +{ + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + USinglebeamSonar(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + /* + * Cleans up octree + */ + virtual void BeginDestroy() override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return RangeBins; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + virtual void showRegion(float DeltaTime) override; + + virtual bool inRange(Octree* tree) override; + + UPROPERTY(EditAnywhere) + float OpeningAngle = 30; + + UPROPERTY(EditAnywhere) + int32 RangeBins = 0; + + UPROPERTY(EditAnywhere) + float RangeRes = 0; + + UPROPERTY(EditAnywhere) + int32 CentralAngleBins = 0; + + UPROPERTY(EditAnywhere) + float CentralAngleRes = 0; + + UPROPERTY(EditAnywhere) + int32 OpeningAngleBins = 0; + + UPROPERTY(EditAnywhere) + float OpeningAngleRes = 0; + +private: + /* + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + */ + AActor* Parent; + + // angles unique to Singlebeam + float CentralAngle = 360; + + float minOpeningAngle; + float maxOpeningAngle; + float minCentralAngle; + float maxCentralAngle; + + // various computations we want to cache + float sqrt3_2; + float sinOffset; + + // Used to hold leafs when parallelized sorting/binning happens + int32* count; + + // for adding noise + MultivariateNormal<1> addNoise; + MultivariateNormal<1> multNoise; + MultivariateUniform<1> rNoise; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/VelocitySensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/VelocitySensor.h new file mode 100644 index 0000000000..9cdc230180 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/VelocitySensor.h @@ -0,0 +1,45 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "VelocitySensor.generated.h" + +/** + * VelocitySensor + * Inherits from the HolodeckSensor class + * Check out the parent class for documentation on all of the overridden funcions. + * Gets the true velocity of the component that the sensor is attached to. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UVelocitySensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UVelocitySensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 3; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + /** + * Parent + * After initialization, Parent contains a pointer to whatever the sensor is attached to. + * Not owned. + */ + UPrimitiveComponent* Parent; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ViewportCapture.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ViewportCapture.h new file mode 100644 index 0000000000..7e775ed3f5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/ViewportCapture.h @@ -0,0 +1,50 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "Holodeck.h" + +#include "HolodeckViewportClient.h" +#include "HolodeckSensor.h" + +#include "ViewportCapture.generated.h" + +UCLASS( ClassGroup=(Custom), meta=(BlueprintSpawnableComponent) ) +class HOLODECK_API UViewportCapture : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UViewportCapture(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + // See HolodeckSensor for documentation for these overridden functions. + int GetNumItems() override { return Width * Height; }; + int GetItemSize() override { return sizeof(FColor); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + UPROPERTY(EditAnywhere) + bool bGrayScale; + + UPROPERTY(EditAnywhere) + int Width; + + UPROPERTY(EditAnywhere) + int Height; + +private: + UHolodeckViewportClient* ViewportClient; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/WorldNumSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/WorldNumSensor.h new file mode 100644 index 0000000000..1ba96af600 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Sensors/Public/WorldNumSensor.h @@ -0,0 +1,41 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "HolodeckCore/Public/HolodeckSensor.h" +#include "WorldNumSensor.generated.h" + +/** +* WorldNumSensor +* Inherits from the HolodeckSensor class +* Check out the parent class for documentation on all of the overridden functions. +* Reports a specific number value (int or float) corresponding to a string key. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UWorldNumSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /* + * Default Constructor + */ + UWorldNumSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + //See HolodeckSensor for the documentation of these overridden functions. + int GetNumItems() override { return 1; }; + int GetItemSize() override { return sizeof(float); }; + void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + FString Key = ""; +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/AvoidTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/AvoidTask.cpp new file mode 100644 index 0000000000..423e88af5f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/AvoidTask.cpp @@ -0,0 +1,80 @@ +#include "Holodeck.h" +#include "AvoidTask.h" +#include "Json.h" + +// Set default values +void UAvoidTask::InitializeSensor() { + Super::InitializeSensor(); +} + +// Allows sensor parameters to be set programmatically from client. +void UAvoidTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("ToAvoid")) { + ToAvoidTag = JsonParsed->GetStringField("ToAvoid"); + } + + if (JsonParsed->HasTypedField("StartSocket")) { + StartSocket = JsonParsed->GetStringField("StartSocket"); + } + + if (JsonParsed->HasTypedField("EndSocket")) { + EndSocket = JsonParsed->GetStringField("EndSocket"); + } + + if (JsonParsed->HasTypedField("OnlyWithinSight")) { + OnlyWithinSight = JsonParsed->GetBoolField("OnlyWithinSight"); + } + + if (JsonParsed->HasTypedField("FOVRadians")) { + FOVRadians = JsonParsed->GetNumberField("FOVRadians"); + } + + if (JsonParsed->HasTypedField("MinDistance")) { + MinDistance = JsonParsed->GetNumberField("MinDistance"); + } + } else { + UE_LOG(LogHolodeck, Warning, TEXT("UAvoidTask::ParseSensorParms:: Unable to parse json.")); + } +} + +// Called every frame +void UAvoidTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (ToAvoid == nullptr && ToAvoidTag != "") + ToAvoid = FindActorWithTag(ToAvoidTag); + + if (ToAvoid) { + // Get location and distance + FVector StartVec = GetActorSocketLocation(ToAvoid, StartSocket); // Parent->GetActorLocation(); + FVector EndVec = GetActorSocketLocation(Parent, EndSocket); + FVector DistanceVec = EndVec - StartVec; + float Distance = DistanceVec.Size(); + + if (OnlyWithinSight) { + bool can_see = IsInSight(ToAvoid, Parent, StartVec, EndVec, FOVRadians, DistanceVec, Distance); + + if (can_see && Distance < MinDistance) { + Reward = MaxScore * Distance / MinDistance; + } + else { + Reward = 100; + } + } + else { + if (Distance < MinDistance) { + Reward = MaxScore * Distance / MinDistance; + } + else { + Reward = 100; + } + } + } + + // Call TaskSensor's Tick to store Reward and Terminal + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CleanUpTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CleanUpTask.cpp new file mode 100644 index 0000000000..275150a15d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CleanUpTask.cpp @@ -0,0 +1,71 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "CleanUpTask.h" + +void UCleanUpTask::InitializeSensor() { + Super::InitializeSensor(); +} + +void UCleanUpTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + + bool HasConfiguration = false; + int NumTrash = 5; + bool UseTable = false; + + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("NumTrash")) { + HasConfiguration = true; + NumTrash = JsonParsed->GetNumberField("NumTrash"); + } + + if (JsonParsed->HasTypedField("UseTable")) { + HasConfiguration = true; + UseTable = JsonParsed->GetBoolField("UseTable"); + } + + // If it does not have a configuration block, it must be set via world command + if (HasConfiguration) { + TArray nums; + nums.Add(NumTrash); + nums.Add(UseTable); + TArray strs; + AActor* Target = GetWorld()->GetAuthGameMode(); + AHolodeckGameMode* Game = static_cast(Target); + Game->ExecuteCustomCommand("CleanUpConfig", nums, strs); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UCleanUpTask::ParseSensorParms:: Unable to parse json.")); + } +} + +void UCleanUpTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + AActor* Target = GetWorld()->GetAuthGameMode(); + AHolodeckGameMode* Game = static_cast(Target); + if (TotalTrash == 0) { + // By default the total trash is zero, so we must keep updating it until it is not zero + TotalTrash = (int)Game->GetWorldNum("TotalTrash"); + } + + int TrashInCan = (int)Game->GetWorldNum("TrashInCan"); + // If trash is added, the reward will be positive and vice versa + Reward = TrashInCan - PrevTick_TrashInCan; + PrevTick_TrashInCan = TrashInCan; + + if (TrashInCan == TotalTrash && TotalTrash != 0) { + Terminal = 1; + } + else { + Terminal = 0; + } + + + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CupGameTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CupGameTask.cpp new file mode 100644 index 0000000000..c2542e5408 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/CupGameTask.cpp @@ -0,0 +1,86 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "HolodeckGameMode.h" +#include "CupGameTask.h" + + +void UCupGameTask::InitializeSensor() { + Super::InitializeSensor(); +} + +// Allows sensor parameters to be set programmatically from client. +void UCupGameTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + + bool HasConfiguration = false; + int32 Speed = 2; + int32 NumShuffles = 3; + bool UseSeed = false; + int32 Seed = 0; + + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("Speed")) { + HasConfiguration = true; + Speed = JsonParsed->GetNumberField("Speed"); + } + + if (JsonParsed->HasTypedField("NumShuffles")) { + HasConfiguration = true; + NumShuffles = JsonParsed->GetNumberField("NumShuffles"); + } + + if (JsonParsed->HasTypedField("Seed")) { + HasConfiguration = true; + UseSeed = true; + Seed = JsonParsed->GetNumberField("Seed"); + } + + if (HasConfiguration){ + TArray nums; + nums.Add(Speed); + nums.Add(NumShuffles); + nums.Add(UseSeed); + nums.Add(Seed); + TArray strs; + strs.Add(AgentName); + AActor* Target = GetWorld()->GetAuthGameMode(); + AHolodeckGameMode* Game = static_cast(Target); + Game->ExecuteCustomCommand("CupGameConfig", nums, strs); + Game->ExecuteCustomCommand("StartCupGame", nums, strs); + } + } + else { + UE_LOG(LogHolodeck, Fatal, TEXT("UCupGameTask::ParseSensorParms:: Unable to parse json.")); + } +} + +void UCupGameTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + AActor* Target = GetWorld()->GetAuthGameMode(); + AHolodeckGameMode* Game = static_cast(Target); + bool BallTouched = Game->GetWorldBool("BallTouched"); + bool CorrectCupTouched = Game->GetWorldBool("CorrectCupTouched"); + bool WrongCupTouched = Game->GetWorldBool("WrongCupTouched"); + + if (BallTouched && !WrongCupTouched) { + Reward = 2; + Terminal = 1; + } + else if (CorrectCupTouched && !WrongCupTouched && !MinRewardGiven){ + Reward = 1; + MinRewardGiven = true; // Only give the min reward once + } + else if (WrongCupTouched) { + Reward = -1; + Terminal = 1; + } + else { + Reward = 0; + } + + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/DistanceTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/DistanceTask.cpp new file mode 100644 index 0000000000..f8854cae09 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/DistanceTask.cpp @@ -0,0 +1,138 @@ +#include "Holodeck.h" +#include "DistanceTask.h" +#include "Json.h" + +// Set default values +void UDistanceTask::InitializeSensor() { + Super::InitializeSensor(); + + + NextDistance = this->ComputeNextDistance(); + +} + +float UDistanceTask::ComputeNextDistance() { + + float Distance = CalcDistance(); + + if (MaximizeDistance) { + return Distance + Interval; + } + else { + return Distance - Interval; + } +} + +// Allows sensor parameters to be set programmatically from client. +void UDistanceTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("DistanceActor")) { + DistanceActorTag = JsonParsed->GetStringField("DistanceActor"); + } + + if (JsonParsed->HasTypedField("GoalActor")) { + GoalActorTag = JsonParsed->GetStringField("GoalActor"); + } + + if (JsonParsed->HasTypedField("GoalLocation")) { + TArray> LocationArray = JsonParsed->GetArrayField("GoalLocation"); + if (LocationArray.Num() == 3) { + double X, Y, Z; + if (LocationArray[0]->TryGetNumber(X) && LocationArray[1]->TryGetNumber(Y) && LocationArray[2]->TryGetNumber(Z)) + GoalLocation = ConvertLinearVector(FVector(X, Y, Z), ClientToUE); + } + } + + if (JsonParsed->HasTypedField("Interval")) { + Interval = ConvertClientDistanceToUnreal(JsonParsed->GetNumberField("Interval")); + + } + + if (JsonParsed->HasTypedField("GoalDistance")) { + GoalDistance = ConvertClientDistanceToUnreal(JsonParsed->GetNumberField("GoalDistance")); + } + + if (JsonParsed->HasTypedField("MaximizeDistance")) { + MaximizeDistance = JsonParsed->GetBoolField("MaximizeDistance"); + } + if (JsonParsed->HasTypedField("3dDistance")) { + UseZ = JsonParsed->GetBoolField("3dDistance"); + } + } else { + UE_LOG(LogHolodeck, Warning, TEXT("UDistanceTask::ParseSensorParms:: Unable to parse json.")); + } +} + +// Called every frame +void UDistanceTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (DistanceActor == nullptr && DistanceActorTag != "") { + DistanceActor = FindActorWithTag(DistanceActorTag); + + if (DistanceActor) { + NextDistance = ComputeNextDistance(); + } + + } + + if (GoalActor == nullptr && GoalActorTag != "") { + GoalActor = FindActorWithTag(GoalActorTag); + + if (GoalActor) { + NextDistance = ComputeNextDistance(); + } + + } + + if ((DistanceActor || DistanceActorTag == "") && (GoalActor || GoalActorTag == "")) { + float Distance = CalcDistance(); + + if (MaximizeDistance) { + if (Distance > NextDistance) { + Reward = 1; + NextDistance = Distance + Interval; + } + else { + Reward = 0; + } + Terminal = Distance > GoalDistance; + } + else { + if (Distance < NextDistance) { + Reward = 1; + NextDistance = Distance - Interval; + } + else { + Reward = 0; + } + Terminal = Distance < GoalDistance; + } + } + + // Call TaskSensor's Tick to store Reward and Terminal in sensor buffer + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} + +// Get Distance +float UDistanceTask::CalcDistance() { + FVector FromLocation = this->GetComponentLocation(); + if (DistanceActor) { + FromLocation = DistanceActor->GetActorLocation(); + } + + FVector ToLocation = GoalLocation; + if (GoalActor) { + ToLocation = GoalActor->GetActorLocation(); + } + + if(!UseZ) { + ToLocation[2] = 0; + FromLocation[2] = 0; + } + + return (ToLocation - FromLocation).Size(); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/FollowTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/FollowTask.cpp new file mode 100644 index 0000000000..11f52f0f6a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/FollowTask.cpp @@ -0,0 +1,72 @@ +#include "Holodeck.h" +#include "FollowTask.h" +#include "Json.h" + +// Set default values +void UFollowTask::InitializeSensor() { + Super::InitializeSensor(); +} + +// Allows sensor parameters to be set programmatically from client. +void UFollowTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("ToFollow")) { + ToFollowTag = JsonParsed->GetStringField("ToFollow"); + } + + if (JsonParsed->HasTypedField("FollowSocket")) { + FollowSocket = JsonParsed->GetStringField("FollowSocket"); + } + + if (JsonParsed->HasTypedField("OnlyWithinSight")) { + OnlyWithinSight = JsonParsed->GetBoolField("OnlyWithinSight"); + } + + if (JsonParsed->HasTypedField("FOVRadians")) { + FOVRadians = JsonParsed->GetNumberField("FOVRadians"); + } + + if (JsonParsed->HasTypedField("MinDistance")) { + MinDistance = ConvertClientDistanceToUnreal(JsonParsed->GetNumberField("MinDistance")); + } + } +} + +// Called every frame +void UFollowTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (ToFollow == nullptr && ToFollowTag != "") + ToFollow = FindActorWithTag(ToFollowTag); + + if (ToFollow) { + // Get location and distance + FVector StartVec = GetActorSocketLocation(Parent, "CameraSocket"); + FVector EndVec = GetActorSocketLocation(ToFollow, FollowSocket); + FVector DistanceVec = EndVec - StartVec; + float Distance = DistanceVec.Size(); + + if (OnlyWithinSight) { + bool can_see = IsInSight(Parent, ToFollow, StartVec, EndVec, FOVRadians, DistanceVec, Distance); + + if (can_see && Distance < MinDistance) { + Reward = MaxScore * (MinDistance - Distance) / MinDistance; + } else { + Reward = 0; + } + } + else { + if (Distance < MinDistance) { + Reward = MaxScore * (MinDistance - Distance) / MinDistance; + } else { + Reward = 0; + } + } + } + + // Call TaskSensor's Tick to store Reward and Terminal + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/LocationTask.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/LocationTask.cpp new file mode 100644 index 0000000000..a3a2b8e866 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/LocationTask.cpp @@ -0,0 +1,96 @@ +#include "Holodeck.h" +#include "LocationTask.h" +#include "Json.h" + +// Set default values +void ULocationTask::InitializeSensor() { + Super::InitializeSensor(); +} + +// Allows sensor parameters to be set programmatically from client. +void ULocationTask::ParseSensorParms(FString ParmsJson) { + Super::ParseSensorParms(ParmsJson); + + TSharedPtr JsonParsed; + TSharedRef> JsonReader = TJsonReaderFactory::Create(ParmsJson); + if (FJsonSerializer::Deserialize(JsonReader, JsonParsed)) { + + if (JsonParsed->HasTypedField("LocationActor")) { + LocationActorTag = JsonParsed->GetStringField("LocationActor"); + } + + if (JsonParsed->HasTypedField("GoalActor")) { + GoalActorTag = JsonParsed->GetStringField("GoalActor"); + } + + if (JsonParsed->HasTypedField("GoalLocation")) { + TArray> LocationArray = JsonParsed->GetArrayField("GoalLocation"); + if (LocationArray.Num() == 3) { + double X, Y, Z; + if (LocationArray[0]->TryGetNumber(X) && LocationArray[1]->TryGetNumber(Y) && LocationArray[2]->TryGetNumber(Z)) + GoalLocation = ConvertLinearVector(FVector(X, Y, Z), ClientToUE); + } + } + + if (JsonParsed->HasTypedField("GoalDistance")) { + GoalDistance = ConvertClientDistanceToUnreal(JsonParsed->GetNumberField("GoalDistance")); + } + + if (JsonParsed->HasTypedField("NegativeReward")) { + NegativeReward = JsonParsed->GetBoolField("NegativeReward"); + } + + if (JsonParsed->HasTypedField("HasTerminal")) { + HasTerminal = JsonParsed->GetBoolField("HasTerminal"); + } + } else { + UE_LOG(LogHolodeck, Warning, TEXT("ULocationTask::ParseSensorParms:: Unable to parse json.")); + } +} + +// Called every frame +void ULocationTask::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (LocationActor == nullptr && LocationActorTag != "") + LocationActor = FindActorWithTag(LocationActorTag); + if (GoalActor == nullptr && GoalActorTag != "") + GoalActor = FindActorWithTag(GoalActorTag); + + if ((LocationActor || LocationActorTag == "") && (GoalActor || GoalActorTag == "")) { + float Distance = CalcDistance(); + + if (Distance < GoalDistance) { + if (!NegativeReward) { + Reward = 1; + } + else { + Reward = -1; + } + + if (HasTerminal) + Terminal = true; + + } + else { + Reward = 0; + Terminal = false; + } + } + + // Call TaskSensor's Tick to store Reward and Terminal in sensor buffer + UTaskSensor::TickSensorComponent(DeltaTime, TickType, ThisTickFunction); +} + +// Get Distance +float ULocationTask::CalcDistance() { + FVector FromLocation = this->GetComponentLocation(); + if (LocationActor) { + FromLocation = LocationActor->GetActorLocation(); + } + + FVector ToLocation = GoalLocation; + if (GoalActor) { + ToLocation = GoalActor->GetActorLocation(); + } + + return (ToLocation - FromLocation).Size(); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskHelper.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskHelper.cpp new file mode 100644 index 0000000000..bb4ac89425 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskHelper.cpp @@ -0,0 +1,27 @@ +#include "Holodeck.h" +#include "TaskHelper.h" + + +FVector GetActorSocketLocation(AActor* actor, FString socket_name) { + FVector StartVec = actor->GetActorLocation(); + UStaticMeshComponent* Mesh = (UStaticMeshComponent*)actor->GetComponentByClass(TSubclassOf()); + if (Mesh && Mesh->DoesSocketExist(FName(*socket_name))) { + StartVec = Mesh->GetSocketLocation(FName(*socket_name)); + } + return StartVec; +} + + +bool IsInSight(AActor* seeing_actor, AActor* target_actor, FVector& start, FVector& end, float FOVRad, FVector& DistanceVec, float Distance) { + // Get angle to target + float TargetAngle = FGenericPlatformMath::Acos(FVector::DotProduct(DistanceVec / Distance, seeing_actor->GetActorForwardVector())); + GEngine->AddOnScreenDebugMessage(-1, 15.0f, FColor::Yellow, FString::Printf(TEXT("Angle %f"), TargetAngle)); + // Get trace to target + FCollisionQueryParams QueryParams = FCollisionQueryParams(); + QueryParams.AddIgnoredActor(seeing_actor); + FHitResult Hit = FHitResult(); + bool TraceResult = seeing_actor->GetWorld()->LineTraceSingleByChannel(Hit, start, end, ECollisionChannel::ECC_Visibility, QueryParams); + + // Evaluate - if the actor is in our field of view and either the ray trace has intersected with the target or there is nothing between ourself and the target + return TargetAngle < FOVRad && (Hit.Actor == target_actor || Hit.Actor == nullptr); +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskSensor.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskSensor.cpp new file mode 100644 index 0000000000..f880198c49 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Private/TaskSensor.cpp @@ -0,0 +1,33 @@ +#include "Holodeck.h" + +#include "TaskSensor.h" +#include "HolodeckGameMode.h" + +// Sets default values +UTaskSensor::UTaskSensor() { + PrimaryComponentTick.bCanEverTick = true; + SensorName = "TaskSensor"; +} + +// Initialize and get Parent +void UTaskSensor::InitializeSensor() { + Super::InitializeSensor(); + //You need to get the pointer to the object you are attached to. + Parent = this->GetAttachmentRootActor(); +} + +// Called every frame +// Sets Reward and Terminal in sensor buffer +// Must be called at the end of child tasks' Tick method +void UTaskSensor::TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) { + if (Parent != nullptr && bOn) { + float* FloatBuffer = static_cast(Buffer); + FloatBuffer[0] = Reward; + FloatBuffer[1] = Terminal; + } +} + +AActor* UTaskSensor::FindActorWithTag(FString tag) { + AHolodeckGameMode* GameTarget = (AHolodeckGameMode*)GetWorld()->GetAuthGameMode(); + return GameTarget->FindActorWithTag(tag); +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/AvoidTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/AvoidTask.h new file mode 100644 index 0000000000..de5680768f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/AvoidTask.h @@ -0,0 +1,77 @@ +#pragma once + +#include "Holodeck.h" + +#include "TaskSensor.h" + +#include "AvoidTask.generated.h" + +/** +* UAvoidTask +* Inherits from the TaskSensor class. +* Calculates Avoid reward based on distance and line of sight. +* If OnlyWithinSight is true, the reward is set to the percent distance covered +* from the MinDistance to the ToAvoid Target if the angle from the agent to the Target +* is less than FOVRadians and is there is nothing blocking the agent's line of sight, +* otherwise the reward is 0. +* If OnlywithinSight is false, the reward is set to the percent distance covered +* from the MinDistance to the ToAvoid Actor. +* Terminal is always false. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UAvoidTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UAvoidTask() : ToAvoid(nullptr), StartSocket(""), EndSocket(""), + OnlyWithinSight(true), FOVRadians(1.5), MinDistance(10000), ToAvoidTag("") {} + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + // Actor to Avoid + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* ToAvoid; + + // Socket on Avoid actor for ray trace + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString StartSocket; + + // Socket on Avoid actor for ray trace + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString EndSocket; + + // Only give reward if target is in sight + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool OnlyWithinSight; + + // Defines the agent's field of view + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float FOVRadians; + + // Defines the minimum distance to recieve positive reward + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MinDistance; + +protected: + //Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + // Scales score between 0-1 to 0-100 + const int MaxScore = 100; + + FString ToAvoidTag; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CleanUpTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CleanUpTask.h new file mode 100644 index 0000000000..23007d557f --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CleanUpTask.h @@ -0,0 +1,43 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "TaskSensor.h" +#include "CleanUpTask.generated.h" + +/** +* UCleanUpTask +* Inherits from the TaskSensor class. +* Initializes the clean up task in the world. This only works in the CleanUp world. +* The reward is based on the number of pieces of trash placed in the trash can. +* For each piece of trash added to the can, a reward of 1 is given. For each piece +* of trash removed, a reward of -1 is given. If all the trash is in the can, terminal +* is given. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UCleanUpTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + // Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + int PrevTick_TrashInCan = 0; + int TotalTrash = 0; +}; \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CupGameTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CupGameTask.h new file mode 100644 index 0000000000..749ec6c78a --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/CupGameTask.h @@ -0,0 +1,43 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +#include "CoreMinimal.h" +#include "TaskSensor.h" +#include "CupGameTask.generated.h" + +/** +* UCupGameTask +* Inherits from the TaskSensor class. +* Initializes the cup game in the world and calculates reward based off of which cup is selected +* A reward of 1 is given for one tick if the correct cup is touched and no other cups are touched, +* A reward of 2 and terminal is given when the ball itself is touched and no incorrect cups are touched. +* A reward of -1 and terminal is given when an incorrect cup is touched. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UCupGameTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + +protected: + // Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + + // Keeps task from giving constant reward when the correct cup is touched. + bool MinRewardGiven = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/DistanceTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/DistanceTask.h new file mode 100644 index 0000000000..dcb75c8d0d --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/DistanceTask.h @@ -0,0 +1,76 @@ +#pragma once + +#include "Holodeck.h" + +#include "TaskSensor.h" + +#include "DistanceTask.generated.h" + +/** +* UDistanceTask +* Inherits from the TaskSensor class. +* Calculates a dense distance based reward. +* Maximizes Distance if Maximize distance is set to true. +* Location is used when actor is null. +* Terminal is set to true when the agent is within its GoalDistance. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UDistanceTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UDistanceTask() : MaximizeDistance(false), Interval(1), GoalDistance(1), GoalActor(nullptr), + GoalLocation(this->GetComponentLocation()), DistanceActor(nullptr), + DistanceActorTag(""), GoalActorTag("") {} + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + // Set to true to maximize instead of minimize distance + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool MaximizeDistance; + + // Distance to next reward + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float Interval; + + // Required proximity for terminal + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float GoalDistance; + + // Goal actor + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* GoalActor; + + // Location (Used if actor is null) + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector GoalLocation; + + // Actor to reach goal (if null component location is used) + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* DistanceActor; + +protected: + // Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + float ComputeNextDistance(); + float CalcDistance(); + float NextDistance; + FString DistanceActorTag; + FString GoalActorTag; + bool UseZ = false; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/FollowTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/FollowTask.h new file mode 100644 index 0000000000..f55bffee77 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/FollowTask.h @@ -0,0 +1,73 @@ +#pragma once + +#include "Holodeck.h" + +#include "TaskSensor.h" + +#include "FollowTask.generated.h" + +/** +* UFollowTask +* Inherits from the TaskSensor class. +* Calculates follow reward based on distance and line of sight. +* If OnlyWithinSight is true, the reward is set to the percent distance covered +* from the MinDistance to the ToFollow Target if the angle from the agent to the Target +* is less than FOVRadians and is there is nothing blocking the agent's line of sight, +* otherwise the reward is 0. +* If OnlywithinSight is false, the reward is set to the percent distance covered +* from the MinDistance to the ToFollow Actor. +* Terminal is always false. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UFollowTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + UFollowTask() : ToFollow(nullptr), FollowSocket(""), OnlyWithinSight(true), + FOVRadians(1.5), MinDistance(10000), ToFollowTag("") {} + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + // Actor to follow + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* ToFollow; + + // Socket on Follow actor for ray trace + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FString FollowSocket; + + // Only give reward if target is in sight + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool OnlyWithinSight; + + // Defines the agent's field of view + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float FOVRadians; + + // Defines the minimum distance to recieve positive reward + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float MinDistance; + +protected: + //Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + // Scales score between 0-1 to 0-100 + const int MaxScore = 100; + + FString ToFollowTag; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/LocationTask.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/LocationTask.h new file mode 100644 index 0000000000..6c1cbd50ed --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/LocationTask.h @@ -0,0 +1,75 @@ +#pragma once + +#include "Holodeck.h" + +#include "TaskSensor.h" + +#include "LocationTask.generated.h" + +/** +* ULocationTask +* Inherits from the TaskSensor class. +* Calculates a sparse distance reward. +* Maximizes Distance if Maximize distance is set to true. +* Location is used when actor is null. +* Terminal is set to true when the agent is within its GoalDistance. +*/ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API ULocationTask : public UTaskSensor +{ + GENERATED_BODY() + +public: + /** + * Default Constructor + */ + ULocationTask() : NegativeReward(false), HasTerminal(true), GoalDistance(100), + GoalActor(nullptr), GoalLocation(this->GetComponentLocation()), LocationActor(nullptr), + LocationActorTag(""), GoalActorTag("") {} + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + + /** + * Allows parameters to be set dynamically + */ + virtual void ParseSensorParms(FString ParmsJson) override; + + // Set to true to maximize instead of minimize distance + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool NegativeReward; + + // Set to true to return a terminal and reward once the goal location is reached + UPROPERTY(EditAnywhere, BlueprintReadWrite) + bool HasTerminal; + + // Required proximity for terminal + UPROPERTY(EditAnywhere, BlueprintReadWrite) + float GoalDistance; + + // Goal actor + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* GoalActor; + + // Location (used if goal actor is null) + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector GoalLocation; + + // Location actor (if null component location is used) + UPROPERTY(EditAnywhere, BlueprintReadWrite) + AActor* LocationActor; + +protected: + //Checkout HolodeckSensor.h for the documentation for this overridden function. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + +private: + + float CalcDistance(); + + FString LocationActorTag; + FString GoalActorTag; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskHelper.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskHelper.h new file mode 100644 index 0000000000..82a82c6953 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskHelper.h @@ -0,0 +1,7 @@ +#pragma once + +#include "Holodeck.h" + +FVector GetActorSocketLocation(AActor* actor, FString socket_name); + +bool IsInSight(AActor* seeing_actor, AActor* target_actor, FVector& start, FVector& end, float FOVRad, FVector& DistanceVec, float Distance); diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskSensor.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskSensor.h new file mode 100644 index 0000000000..3536274efa --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Tasks/Public/TaskSensor.h @@ -0,0 +1,55 @@ +#pragma once + +#include "Holodeck.h" + +#include "HolodeckSensor.h" + +#include "TaskHelper.h" + +#include "TaskSensor.generated.h" + +/** + * UTaskSensor + * A base class for tasks within Holodeck. + * This class is a Holodeck Sensor. + * This class assigns a reward to the HolodeckAgent it's attached to. + * The task logic then sets the reward and terminal each tick. + * The child class must remember to call the parent tick class at the end of + * setting the reward and terminal. This allows the parent class to set those + * variables in the shared memory. + */ +UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent)) +class HOLODECK_API UTaskSensor : public UHolodeckSensor { + GENERATED_BODY() + +public: + /** + * Default Constructor. + */ + UTaskSensor(); + + /** + * InitializeSensor + * Sets up the class + */ + virtual void InitializeSensor() override; + +protected: + // See HolodeckSensor.h for the documentation of these overridden functions. + // Must be overriden by child task. + virtual void TickSensorComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override; + + // Child tasks should not need to override these three methods + int GetNumItems() override { return 2; }; + int GetItemSize() override { return sizeof(float); }; + + // Find actor with tag + AActor* FindActorWithTag(FString tag); + + // After initialization, Parent contains a pointer to whatever the sensor is attached to. + AActor* Parent; + + // Members to be set by child tasks each tick + float Reward; + bool Terminal; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/Benchmarker.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/Benchmarker.cpp new file mode 100644 index 0000000000..91d360bf25 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/Benchmarker.cpp @@ -0,0 +1,31 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "Benchmarker.h" + +Benchmarker::Benchmarker(bool manual_start_) : manual_start(manual_start_) +{ + t_start = std::chrono::high_resolution_clock::now(); +} + +Benchmarker::~Benchmarker() +{ +} + +void Benchmarker::Start() +{ + t_start = std::chrono::high_resolution_clock::now(); +} + +void Benchmarker::End() +{ + t_end = std::chrono::high_resolution_clock::now(); +} + +float Benchmarker::CalcMs() +{ + if(!manual_start) t_end = std::chrono::high_resolution_clock::now(); + float duration = std::chrono::duration(t_end - t_start).count(); + if(!manual_start) t_start = std::chrono::high_resolution_clock::now(); + return duration; +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/EasyFileManager.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/EasyFileManager.cpp new file mode 100644 index 0000000000..e7906d64d8 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/EasyFileManager.cpp @@ -0,0 +1,26 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#include "Holodeck.h" +#include "EasyFileManager.h" + +EasyFileManager::EasyFileManager() +{ +} + +EasyFileManager::~EasyFileManager() +{ +} + +void EasyFileManager::SaveToFile(FString Data, FString FileName, FString Path, bool bOverwrite) +{ + IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); + + if (PlatformFile.CreateDirectoryTree(*Path)) + { + FString AbsoluteFilePath = Path + FileName; + if (bOverwrite || !PlatformFile.FileExists(*AbsoluteFilePath)) + { + FFileHelper::SaveStringToFile(Data, *AbsoluteFilePath); + } + } +} diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/gason.cpp b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/gason.cpp new file mode 100644 index 0000000000..2a08f1f8a5 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Private/gason.cpp @@ -0,0 +1,475 @@ +#include "gason.h" +#include +#if defined(GASON_DEBUG_ALLOCATOR) +# include +#endif +/////////////////////////////////////////////////////////////////////////////// +namespace gason { +/////////////////////////////////////////////////////////////////////////////// +#define JSON_ZONE_SIZE 4096 +#define JSON_STACK_SIZE 32 + +struct JsonAllocator::Zone { +protected: + Zone* prev; + Zone* next; + size_t used; + + friend class JsonAllocator; + +public: + static Zone* create(size_t allocSize) { + assert(allocSize <= JSON_ZONE_SIZE); + Zone* z = (Zone*) ::malloc(JSON_ZONE_SIZE); + assert( z != 0 ); + z->used = allocSize; + z->next = nullptr; + z->prev = nullptr; + + return z; + } + +protected: + static Zone* begin(Zone* head) { + Zone* iter = head; + while ( iter ) { + Zone* prev = iter->prev; + if ( prev == nullptr ) + break; + iter = prev; + } + + return iter; + } + + static Zone* end(Zone* head) { + Zone* iter = head; + while ( iter ) { + Zone* next = iter->next; + if ( next == nullptr ) + break; + iter = next; + } + + return iter; + } + + static size_t totalAllocatedSpace(Zone* head) { + size_t total = 0; + Zone* iter = begin(head); + + while ( iter ) { + total += JSON_ZONE_SIZE; + Zone* next = iter->next; + iter = next; + } + + return total; + } +}; + +void* +JsonAllocator::allocate(size_t size) { + size = (size + 7) & ~7; // ensure the size = 8n and 8n >= size + + if ( head ) { // head is a valid pre-allocated + assert( head->used <= JSON_ZONE_SIZE ); + + if ( head->used + size <= JSON_ZONE_SIZE ) { // head has enough empty spaces + char *p = (char*)head + head->used; + head->used += size; + return p; + } + + else if ( head->next ) { // head if full, go for next valid Zone + Zone* zone = head->next; + assert(zone->used == sizeof(Zone)); + assert(zone->prev == head); + zone->used += size; + head = zone; + return (char*)zone + sizeof(Zone); + } + } + + + size_t allocSize = sizeof(Zone) + size; + Zone* zone = Zone::create(allocSize); + if ( zone == nullptr ) + return nullptr; + + zone->prev = head; +#ifdef GASON_DEBUG_ALLOCATOR + fprintf(stderr, "allocating %p (prev: %p , next: %p)\n", zone, zone->prev, zone->next); +#endif + if ( head ) + head->next = zone; + head = zone; + + return (char*)zone + sizeof(Zone); +} + +void +JsonAllocator::deallocate() { +#ifdef GASON_DEBUG_ALLOCATOR + size_t totalUsedSpace = Zone::totalAllocatedSpace(head); + fprintf(stderr, "\n%s(%d): %lu bytes are going to be deallocated.\n", + __FILE__, __LINE__, + totalUsedSpace); +#endif + + // goto last allocated item + Zone* iter = Zone::end(head); + + while ( iter ) { +#ifdef GASON_DEBUG_ALLOCATOR + fprintf(stderr, "freeing %p (prev: %p , next: %p)\n", iter, iter->prev, iter->next); +#endif + Zone *prev = iter->prev; + free(iter); + iter = prev; + } + + head = nullptr; +} + +void +JsonAllocator::reset() { + Zone* iter = Zone::end(head); + + while ( iter ) { + memset((char*)iter + sizeof(Zone), 0, JSON_ZONE_SIZE - sizeof(Zone)); + iter->used = sizeof(Zone); + Zone* prev = iter->prev; + if ( prev == nullptr ) // break if head is the first allocated Zone. + break; + iter = prev; + } + + head = iter; +} + +static inline bool +isspace(char c) { + return c == ' ' || (c >= '\t' && c <= '\r'); +} + +static inline bool +isdelim(char c) { + return c == ',' || c == ':' || c == ']' || c == '}' || isspace(c) || !c; +} + +static inline bool +isdigit(char c) { + return c >= '0' && c <= '9'; +} + +static inline bool +isxdigit(char c) { + return (c >= '0' && c <= '9') || ((c & ~' ') >= 'A' && (c & ~' ') <= 'F'); +} + +static inline int +char2int(char c) { + if (c <= '9') + return c - '0'; + return (c & ~' ') - 'A' + 10; +} + +static double +string2double(char *s, char **endptr) { + char ch = *s; + if (ch == '-') + ++s; + + double result = 0; + while (isdigit(*s)) + result = (result * 10) + (*s++ - '0'); + + if (*s == '.') { + ++s; + + double fraction = 1; + while (isdigit(*s)) { + fraction *= 0.1; + result += (*s++ - '0') * fraction; + } + } + + if (*s == 'e' || *s == 'E') { + ++s; + + double base = 10; + if (*s == '+') + ++s; + else if (*s == '-') { + ++s; + base = 0.1; + } + + unsigned int exponent = 0; + while (isdigit(*s)) + exponent = (exponent * 10) + (*s++ - '0'); + + double power = 1; + for (; exponent; exponent >>= 1, base *= base) + if (exponent & 1) + power *= base; + + result *= power; + } + + *endptr = s; + return ch == '-' ? -result : result; +} + +static inline JsonNode* +insertAfter(JsonNode *tail, JsonNode *node) { + if (!tail) + return node->next = node; + node->next = tail->next; + tail->next = node; + return node; +} + +static inline JsonValue +listToValue(JsonTag tag, JsonNode *tail) { + if (tail) { + JsonNode* head = tail->next; + tail->next = nullptr; + return JsonValue(tag, head); + } + return JsonValue(tag, nullptr); +} + +JsonParseStatus +jsonParse(char *s, char **endptr, JsonValue *value, JsonAllocator &allocator) { + JsonNode *tails[JSON_STACK_SIZE]; + JsonTag tags[JSON_STACK_SIZE]; + char *keys[JSON_STACK_SIZE]; + JsonValue o; + int pos = -1; + + // reset allocator Zones. + allocator.reset(); + + bool separator = true; + + *endptr = s; + + while (*s) { + while (isspace(*s)) { + ++s; + if (!*s) break; + } + *endptr = s++; + switch (**endptr) { + case '-': + if (!isdigit(*s) && *s != '.') { + *endptr = s; + return JSON_PARSE_BAD_NUMBER; + } + case '0': + case '1': + case '2': + case '3': + case '4': + case '5': + case '6': + case '7': + case '8': + case '9': + o = JsonValue(string2double(*endptr, &s)); + if (!isdelim(*s)) { + *endptr = s; + return JSON_PARSE_BAD_NUMBER; + } + break; + case '"': + o = JsonValue(JSON_STRING, s); + for (char *it = s; *s; ++it, ++s) { + int c = *it = *s; + if (c == '\\') { + c = *++s; + switch (c) { + case '\\': + case '"': + case '/': + *it = c; + break; + case 'b': + *it = '\b'; + break; + case 'f': + *it = '\f'; + break; + case 'n': + *it = '\n'; + break; + case 'r': + *it = '\r'; + break; + case 't': + *it = '\t'; + break; + case 'u': + c = 0; + for (int i = 0; i < 4; ++i) { + if ( isxdigit(*++s)) { + c = c * 16 + char2int(*s); + } else { + *endptr = s; + return JSON_PARSE_BAD_STRING; + } + } + if (c < 0x80) { + *it = c; + } else if (c < 0x800) { + *it++ = 0xC0 | (c >> 6); + *it = 0x80 | (c & 0x3F); + } else { + *it++ = 0xE0 | (c >> 12); + *it++ = 0x80 | ((c >> 6) & 0x3F); + *it = 0x80 | (c & 0x3F); + } + break; + default: + *endptr = s; + return JSON_PARSE_BAD_STRING; + } + } else if ((unsigned int)c < ' ' || c == '\x7F') { + *endptr = s; + return JSON_PARSE_BAD_STRING; + } else if (c == '"') { + *it = 0; + ++s; + break; + } + } + if (!isdelim(*s)) { + *endptr = s; + return JSON_PARSE_BAD_STRING; + } + break; + case 't': + if (!(s[0] == 'r' && s[1] == 'u' && s[2] == 'e' && isdelim(s[3]))) + return JSON_PARSE_BAD_IDENTIFIER; + o = JsonValue(JSON_TRUE); + s += 3; + break; + case 'f': + if (!(s[0] == 'a' && s[1] == 'l' && s[2] == 's' && s[3] == 'e' && isdelim(s[4]))) + return JSON_PARSE_BAD_IDENTIFIER; + o = JsonValue(JSON_FALSE); + s += 4; + break; + case 'n': + if (!(s[0] == 'u' && s[1] == 'l' && s[2] == 'l' && isdelim(s[3]))) + return JSON_PARSE_BAD_IDENTIFIER; + o = JsonValue(JSON_FALSE); + s += 4; + break; + case ']': + if (pos == -1) + return JSON_PARSE_STACK_UNDERFLOW; + if (tags[pos] != JSON_ARRAY) + return JSON_PARSE_MISMATCH_BRACKET; + o = listToValue(JSON_ARRAY, tails[pos--]); + break; + case '}': + if (pos == -1) + return JSON_PARSE_STACK_UNDERFLOW; + if (tags[pos] != JSON_OBJECT) + return JSON_PARSE_MISMATCH_BRACKET; + if (keys[pos] != nullptr) + return JSON_PARSE_UNEXPECTED_CHARACTER; + o = listToValue(JSON_OBJECT, tails[pos--]); + break; + case '[': + if (++pos == JSON_STACK_SIZE) + return JSON_PARSE_STACK_OVERFLOW; + tails[pos] = nullptr; + tags[pos] = JSON_ARRAY; + keys[pos] = nullptr; + separator = true; + continue; + case '{': + if (++pos == JSON_STACK_SIZE) + return JSON_PARSE_STACK_OVERFLOW; + tails[pos] = nullptr; + tags[pos] = JSON_OBJECT; + keys[pos] = nullptr; + separator = true; + continue; + case ':': + if (separator || keys[pos] == nullptr) + return JSON_PARSE_UNEXPECTED_CHARACTER; + separator = true; + continue; + case ',': + if (separator || keys[pos] != nullptr) + return JSON_PARSE_UNEXPECTED_CHARACTER; + separator = true; + continue; + case '\0': + continue; + default: + return JSON_PARSE_UNEXPECTED_CHARACTER; + } + + separator = false; + + if (pos == -1) { + *endptr = s; + *value = o; + return JSON_PARSE_OK; + } + + if (tags[pos] == JSON_OBJECT) { + if (!keys[pos]) { + if (o.getTag() != JSON_STRING) + return JSON_PARSE_UNQUOTED_KEY; + keys[pos] = o.toString(); + continue; + } + JsonNode* node = (JsonNode*) allocator.allocate(sizeof(JsonNode)); + if ( node == nullptr ) + return JSON_PARSE_ALLOCATION_FAILURE; + tails[pos] = insertAfter(tails[pos], node); + tails[pos]->key = keys[pos]; + keys[pos] = nullptr; + } else { + JsonNode* node = (JsonNode*) allocator.allocate(sizeof(JsonNode) - sizeof(char*)); + if ( node == nullptr ) + return JSON_PARSE_ALLOCATION_FAILURE; + tails[pos] = insertAfter(tails[pos], node); + } + tails[pos]->value = o; + } + return JSON_PARSE_BREAKING_BAD; +} + +JsonValue +JsonValue::child(const char* key) const { + for ( JsonIterator it = begin(*this); it != end(*this); it++) { + if ( strncmp(it->key, key, strlen(key)) == 0 ) + return it->value; + } + + return JsonValue(); +} + +JsonValue +JsonValue::at(size_t index) const { + if ( getTag() == JSON_ARRAY ) { + size_t i = 0; + for ( JsonIterator it = begin(*this); it != end(*this); it++) { + if ( i++ == index ) + return it->value; + } + } + + return JsonValue(); +} +/////////////////////////////////////////////////////////////////////////////// +} // namespace gason +/////////////////////////////////////////////////////////////////////////////// diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/Benchmarker.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/Benchmarker.h new file mode 100644 index 0000000000..74a5946577 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/Benchmarker.h @@ -0,0 +1,42 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once +#include +#include + +/** + * + */ +class HOLODECK_API Benchmarker +{ +public: + // If manual start is false, CalcMs will automatically end your iteration and start the next one. + Benchmarker(bool manual_start_=false); + ~Benchmarker(); + + /** + * Start + * + * Start before the function you want to benchmark + */ + void Start(); + + /** + * End + * + * End after the function you want to benchmark + */ + void End(); + + /** + * CalculateAvg + * + * Calculates the total time for the function + */ + float CalcMs(); + +private: + bool manual_start; + std::chrono::high_resolution_clock::time_point t_start; + std::chrono::high_resolution_clock::time_point t_end; +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/EasyFileManager.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/EasyFileManager.h new file mode 100644 index 0000000000..1762735bd3 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/EasyFileManager.h @@ -0,0 +1,15 @@ +// MIT License (c) 2019 BYU PCCL see LICENSE file + +#pragma once + +/** + * + */ +class HOLODECK_API EasyFileManager +{ +public: + EasyFileManager(); + ~EasyFileManager(); + + void SaveToFile(FString Data, FString FileName, FString Path = "C:\\Users\\robert.pottorff\\Desktop\\", bool bOverwrite = true); +}; diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/gason.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/gason.h new file mode 100644 index 0000000000..893a13fc6b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/gason.h @@ -0,0 +1,314 @@ +/** + * @file gason.hpp + * a simple and fast JSon parser in plain C/C++ with no dependency. + * + * + * @author Ivan Vashchaev + * @version 1.0.0 + * @date 2014-07-07 + * based on this commit: ede29fc + * https://github.com/vivkin/gason + * + * @author amir zamani + * @version 2.1.0 + * @date 2014-07-11 + * https://github.com/azadkuh/gason-- + * + * @author amir zamani + * @version 2.2.0 + * @date 2014-11-24 + * add latest features from gason/#d09cd7a + * + */ + +#ifndef __GASON_HPP__ +#define __GASON_HPP__ +/////////////////////////////////////////////////////////////////////////////// +#include +#include +#include +#include +/////////////////////////////////////////////////////////////////////////////// +namespace gason { +/////////////////////////////////////////////////////////////////////////////// +#if __cplusplus <= 199711L +# define GASON_OLDTOOLCHAINS_SUPPORT +#endif + +#if defined(GASON_OLDTOOLCHAINS_SUPPORT) +# ifndef nullptr +# define nullptr NULL +# endif +#endif // GASON_OLDTOOLCHAINS_SUPPORT +/////////////////////////////////////////////////////////////////////////////// +/** tag (type) of each JSon element. */ +enum JsonTag { + JSON_NUMBER = 0, ///< double (floating point) value + JSON_STRING, ///< string value + JSON_ARRAY, ///< an array value + JSON_OBJECT, ///< an object value + JSON_TRUE, ///< true value + JSON_FALSE, ///< false value + JSON_NULL = 0xF ///< null or invalid value +}; +/////////////////////////////////////////////////////////////////////////////// +struct JsonNode; +/////////////////////////////////////////////////////////////////////////////// +/** JSon value of @sa JsonTag type. */ +struct JsonValue { + union { + uint64_t ival; + double fval; + }; + + JsonValue(double x) : fval(x) { + } + JsonValue(JsonTag tag = JSON_NULL, void *payload = nullptr) { + assert((uintptr_t)payload <= JSON_VALUE_PAYLOAD_MASK); + ival = JSON_VALUE_NAN_MASK | ((uint64_t)tag << JSON_VALUE_TAG_SHIFT) | (uintptr_t)payload; + } + + bool isNumber() const { + return getTag() == JSON_NUMBER; + } + bool isBoolean() const { + return getTag() == JSON_TRUE || getTag() == JSON_FALSE; + } + bool isString() const { + return getTag() == JSON_STRING; + } + bool isNode() const { + return getTag() == JSON_ARRAY || getTag() == JSON_OBJECT; + } + bool isArray() const { + return getTag() == JSON_ARRAY; + } + bool isObject() const { + return getTag() == JSON_OBJECT; + } + bool isDouble() const { + return (int64_t)ival <= (int64_t)JSON_VALUE_NAN_MASK; + } + JsonTag getTag() const { + return isDouble() ? JSON_NUMBER : JsonTag((ival >> JSON_VALUE_TAG_SHIFT) & JSON_VALUE_TAG_MASK); + } + + int toInt(bool* ok = nullptr) const { + return (int) toNumber(ok); + } + double toNumber(bool* ok = nullptr) const { + if ( !checkType(isNumber(), ok) ) + return 0.0; + + return fval; + } + bool toBool(bool* ok = nullptr) const { + if ( !checkType(isBoolean(), ok) ) + return false; + + return getPayload() == JSON_TRUE; + } + char* toString(bool* ok = nullptr) const { + if ( !checkType(isString(), ok) ) + return nullptr; + + return (char *)getPayload(); + } + JsonNode* toNode(bool* ok = nullptr) const { + if ( !checkType(isNode(), ok) ) + return nullptr; + + return (JsonNode *)getPayload(); + } + + /** returns true if this object is not NULL. */ + operator bool()const { + return getTag() != JSON_NULL; + } + /** returns true if this object has typeof tag value. */ + bool operator==(JsonTag tag) const { + return getTag() == tag; + } + /** returns true if this object is not typeof tag value. */ + bool operator!=(JsonTag tag) const { + return getTag() != tag; + } + + /** overloads @sa at. */ + JsonValue operator[](size_t index) const { + return at(index); + } + /** overloads @sa child. */ + JsonValue operator()(const char* keyName) const { + return child(keyName); + } + /** returns a child value associated with the key = keyName. */ + JsonValue child(const char* keyName) const; + /** returns the item at index position i in the array. */ + JsonValue at(size_t i) const; + +protected: + uint64_t getPayload() const { + assert(!isDouble()); + return ival & JSON_VALUE_PAYLOAD_MASK; + } + + bool checkType(bool isMatched, bool *ok) const { + if ( ok == 0 ) { + assert(isMatched && "failed to convert a JsonValue because of type difference."); + return true; + } + + *ok = isMatched; + return isMatched; + } + + static const uint64_t JSON_VALUE_PAYLOAD_MASK = 0x00007FFFFFFFFFFFULL; + static const uint64_t JSON_VALUE_NAN_MASK = 0x7FF8000000000000ULL; + static const uint64_t JSON_VALUE_NULL = 0x7FFF800000000000ULL; + static const uint64_t JSON_VALUE_TAG_MASK = 0xF; + static const uint64_t JSON_VALUE_TAG_SHIFT = 47; +}; +/////////////////////////////////////////////////////////////////////////////// +/** a JsonNode is a JsonValue who is an array or object. */ +struct JsonNode { + JsonValue value; + JsonNode* next; + char* key; +}; +/////////////////////////////////////////////////////////////////////////////// +struct JsonIterator { + JsonNode* p; + + explicit JsonIterator(JsonNode* n = nullptr) : p(n) { + } + + void operator++() { + p = p->next; + } + void operator++(int) { + p = p->next; + } + + bool isValid()const { + return p != nullptr; + } + bool hasNext()const { + return p->next != nullptr; + } + + bool operator==(const char* key) const { + return strncmp(p->key, key, strlen(key)) == 0; + } + bool operator!=(const JsonIterator &x) const { + return p != x.p; + } + + JsonNode* operator*() const { + return p; + } + JsonNode* operator->() const { + return p; + } +}; + +inline JsonIterator begin(JsonValue o) { + bool bok; + return JsonIterator(o.toNode(&bok)); +} +inline JsonIterator end(JsonValue) { + return JsonIterator(nullptr); +} +/////////////////////////////////////////////////////////////////////////////// + +enum JsonParseStatus { + JSON_PARSE_OK, + JSON_PARSE_BAD_NUMBER, + JSON_PARSE_BAD_STRING, + JSON_PARSE_BAD_IDENTIFIER, + JSON_PARSE_STACK_OVERFLOW, + JSON_PARSE_STACK_UNDERFLOW, + JSON_PARSE_MISMATCH_BRACKET, + JSON_PARSE_UNEXPECTED_CHARACTER, + JSON_PARSE_UNQUOTED_KEY, + JSON_PARSE_BREAKING_BAD, + JSON_PARSE_ALLOCATION_FAILURE +}; + +/** Automatic memory manager for parsed JsonValue. + * @sa jsonParse(). + * used internally by jsonParse(). you do not need to manually handle this object. + * just keep allocator instance as long as you want to keep JsonValues. + */ +class JsonAllocator { +public: + JsonAllocator() : head(nullptr) { + } + ~JsonAllocator() { + deallocate(); + } + + /** frees the allocated memory. + * you do not need to call this function. If for some reason you have to + * release the memory manually, beware that after deallocate(), + * all the parsed JsonValues are invalid. + */ + void deallocate(); + +#if defined(GASON_OLDTOOLCHAINS_SUPPORT) +private: + JsonAllocator(const JsonAllocator &); + JsonAllocator &operator=(const JsonAllocator &); +#else + JsonAllocator(const JsonAllocator &) = delete; + JsonAllocator &operator=(const JsonAllocator &) = delete; + JsonAllocator(JsonAllocator &&x) : head(x.head) { + x.head = nullptr; + } + JsonAllocator &operator=(JsonAllocator &&x) { + head = x.head; + x.head = nullptr; + return *this; + } +#endif + +protected: + struct Zone; + Zone* head; + + void* allocate(size_t size); + void reset(); + + friend JsonParseStatus jsonParse(char*, char**, JsonValue*, JsonAllocator&); +}; + +JsonParseStatus +jsonParse(char *str, char **endptr, JsonValue *value, JsonAllocator &allocator); + +/** parses a JSon string and return root object or array. + * JsonValues are the index tree for actual data on jsonString. allocator object holds + * and manage these indices. + * + * to keep the JsonValue valid, you have to keep both jsonString (data) + * and allocator (instances) alive. + * + * to avoid calling alloc/free many times (a serious problem on embedded devices) + * the allocator is re-usable and you can hold and re-use instance for parsing + * as many as jsonStrings you want. JsonAllocator automatically expands + * if more memory is required and frees all memories automatically. + * + * @param jsonString the JSon string. this buffer will be modified by this function. + * @param root root object of JSon string. could be an object or array. + * @param allocator an instance to memory manager. + * this function calls allocator.reset() before parsing. + * @return returns JSON_PARSE_OK or a proper error code. + */ +inline JsonParseStatus +jsonParse(char* jsonString, JsonValue& root, JsonAllocator& allocator) { + char *endptr = nullptr; + return jsonParse(jsonString, &endptr, &root, allocator); +} +/////////////////////////////////////////////////////////////////////////////// +} // namespace gason +/////////////////////////////////////////////////////////////////////////////// +#endif // __GASON_HPP__ diff --git a/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/jsonbuilder.h b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/jsonbuilder.h new file mode 100644 index 0000000000..4d9ef18970 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/HoloOcean/Source/Holodeck/Utils/Public/jsonbuilder.h @@ -0,0 +1,272 @@ +/** + * @file jsonbuilder.hpp + * a simple and fast JSon generator in plain C/C++ with no dependency. + * + * + * @author amir zamani + * @version 1.0.0 + * @date 2014-05-14 + * + * @author amir zamani + * @version 1.0.1 + * @date 2014-05-16 + * new method : addNull and addValue (unnamed) + * + */ +#ifndef _JSONBUILDER_HPP__ +#define _JSONBUILDER_HPP__ + +#include + +/////////////////////////////////////////////////////////////////////////////// +namespace gason { +/////////////////////////////////////////////////////////////////////////////// +/** a string builder class. + * mainly used as a base for JSonBuilder. this class does not own the buffer. + */ +class StringBuilder +{ +public: + /** default ctor + * @param buffer result buffer to hold builded string. + * @param length length (capacity of buffer). + */ + explicit StringBuilder(char* buffer, size_t length) + : ibuffer(buffer), ilength(length), iindex(0) { + reset(); + } + + /** resets the string object. (writes NULL into buffer). */ + void reset() { + memset(ibuffer, 0, ilength); + iindex = 0; + } + + /** returns the empty spaces left. return 0 if the buffer is full. */ + size_t emptySpaces() const { + return ( iindex < ilength ) ? (ilength - 1 - iindex) : 0; + } + + StringBuilder& append(const char* string, size_t length=0) { + if ( length == 0 ) + length = strlen(string); + + if ( ( length + iindex ) < ilength ) { + strncat(ibuffer+iindex, string, length); + iindex += length; + } + return *this; + } + + StringBuilder& operator << (const char* string) { + return append(string); + } + + StringBuilder& operator << (int number) { + int length = snprintf(ibuffer+iindex, + ilength-iindex, + "%d", number); + if ( length > 0 ) + iindex += length; + + return *this; + } + + StringBuilder& operator << (double number) { + int length = snprintf(ibuffer+iindex, + ilength-iindex, + "%lf", number); + if ( length > 0 ) + iindex += length; + + return *this; + } + +#if defined(QBYTEARRAY_H) + StringBuilder& operator << (const QByteArray& bytes) { + return append(bytes.constData()); + } +#endif + +#if defined(QSTRING_H) + StringBuilder& operator << (const QString& string) { + return append(string.toUtf8().constData()); + } +#endif + +protected: + char* ibuffer; ///< internal pointer to the start of the buffer. + size_t ilength; ///< length (capacity) of buffer. + size_t iindex; ///< current writing index. +}; +/////////////////////////////////////////////////////////////////////////////// +/** JSonBuilder. uses @sa StringBuilder as backend. + */ +class JSonBuilder : protected StringBuilder +{ +public: + /** default ctor + * @param buffer result buffer to hold builded string. + * @param length length (capacity of buffer). + */ + explicit JSonBuilder(char *buffer, size_t length) : StringBuilder(buffer, length) { + } + + using StringBuilder::reset; + using StringBuilder::emptySpaces; + + /** checks if the specified buffer is not small and holds the proper result json string. */ + bool isBufferAdequate() const { + return emptySpaces() > 0; + } + + /** starts an object by @code { @endcode or @code "name" : { @endcode if name is not empty. */ + JSonBuilder& startObject(const char* name = 0) { + addPossibleComma(); + if ( name != 0 && strlen(name) > 0 ) { + *this << "\"" << name << "\":"; + } + + operator <<("{"); + return *this; + } + + /** ends an object by @code } @endcode. */ + JSonBuilder& endObject() { + operator <<("}"); + return *this; + } + + /** ends an object by @code , @endcode. */ + JSonBuilder& comma() { + operator <<(","); + return *this; + } + + /** starts an array by @code [ @endcode or @code "name" : [ @endcode if name is not empty. */ + JSonBuilder& startArray(const char* name = 0) { + addPossibleComma(); + if ( name != 0 && strlen(name) > 0 ) { + *this << "\"" << name << "\":"; + } + + operator <<("["); + return *this; + } + + /** ends an array by @code ] @endcode. */ + JSonBuilder& endArray() { + operator <<("]"); + return *this; + } + + /** writes a value by @code "name" : "value" @endcode. */ + JSonBuilder& addValue(const char* name, const char* value) { + addPossibleComma() << "\"" << name << "\":\"" << value << "\""; + return *this; + } + + /** writes a value by @code "name" : number @endcode. */ + JSonBuilder& addValue(const char *name, int number) { + addPossibleComma() << "\"" << name << "\":" << number; + return *this; + } + + /** writes a value by @code "name" : number @endcode. */ + JSonBuilder& addValue(const char *name, size_t number) { + addPossibleComma() << "\"" << name << "\":" << (int) number; + return *this; + } + + /** writes a value by @code "name" : number @endcode. */ + JSonBuilder& addValue(const char *name, double number) { + addPossibleComma() << "\"" << name << "\":" << number; + return *this; + } + + /** writes a value by @code "name" : true/false @endcode. */ + JSonBuilder& addValue(const char *name, bool state) { + addPossibleComma() << "\"" << name << "\":" << ((state) ? "true":"false"); + return *this; + } + + /** writes a null value by @code "name" : null @endcode. */ + JSonBuilder& addNull(const char *name) { + addPossibleComma() << "\"" << name << "\":" << "null"; + return *this; + } + + // for unnamed elements + /** writes an unnamed value by @code "value" @endcode. */ + JSonBuilder& addValue(const char* value) { + addPossibleComma() << "\"" << value << "\""; + return *this; + } + + /** writes a unnamed value by @code number @endcode. */ + JSonBuilder& addValue(int number) { + addPossibleComma() << number; + return *this; + } + + /** writes a unnamed value by @code number @endcode. */ + JSonBuilder& addValue(size_t number) { + addPossibleComma() << (int) number; + return *this; + } + + /** writes a unnamed value by @code number @endcode. */ + JSonBuilder& addValue(double number) { + addPossibleComma() << number; + return *this; + } + + /** writes a unnamed value by @code true/false @endcode. */ + JSonBuilder& addValue(bool state) { + addPossibleComma() << ((state) ? "true":"false"); + return *this; + } + + /** writes a null value by @code null @endcode. */ + JSonBuilder& addNull() { + addPossibleComma() << "null"; + return *this; + } + + +#if defined(QBYTEARRAY_H) + /** writes a value by @code "name" : "value" @endcode. */ + JSonBuilder& addValue(const char *name, const QByteArray& value) { + addPossibleComma() << "\"" << name << "\":\"" << value << "\""; + return *this; + } +#endif + +#if defined(QSTRING_H) + /** writes a value by @code "name" : "value" @endcode. */ + JSonBuilder& addValue(const char *name, const QString& value) { + addPossibleComma() << "\"" << name << "\":\"" << value << "\""; + return *this; + } +#endif + +protected: + JSonBuilder& addPossibleComma() { + if ( iindex > 0 ) { + char prev = ibuffer[iindex-1]; + if ( prev != '{' && prev != '[' && prev != ',' ) { + *this << ","; + } + } + + return *this; + } + +}; + + + +/////////////////////////////////////////////////////////////////////////////// +} // namespace gason +/////////////////////////////////////////////////////////////////////////////// +#endif // _JSONBUILDER_HPP__ diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk index 948e78bd8c..96a3016590 100644 --- a/Util/BuildTools/Windows.mk +++ b/Util/BuildTools/Windows.mk @@ -14,7 +14,7 @@ export git_code=T8w6TYB_r71gGTP3A02B export INSTALLATION_DIR=$(ROOT_PATH)Build/ export BOOST_VERSION=1.86.0 -export API_VERSION=2.10.0 +export API_VERSION=2.10.1 export BOOST_INSTALL_FOLDER=${INSTALLATION_DIR}boost-${BOOST_VERSION}-install/ export BOOST_SOURCE_FOLDER=${INSTALLATION_DIR}boost-${BOOST_VERSION}-source/ diff --git a/Util/Docker/holoocean/Dockerfile b/Util/Docker/holoocean/Dockerfile new file mode 100644 index 0000000000..f7c6c5461e --- /dev/null +++ b/Util/Docker/holoocean/Dockerfile @@ -0,0 +1,29 @@ +FROM adamrehn/ue4-runtime:22.04-cudagl11-noaudio + +USER root +ENV DEBIAN_FRONTEND=noninteractive +RUN apt-key adv --fetch-keys https://developer.download.nvidia.com/compute/cuda/repos/ubuntu2004/x86_64/3bf863cc.pub +RUN apt-get update && apt-get install -y --no-install-recommends \ + curl git libglib2.0-dev software-properties-common + +# OpenCV's runtime dependencies (and other dependencies) +RUN apt-get install -y libglib2.0-0 libsm6 libxrender-dev libxext6 + +# Install all python versions to test on +RUN apt-get update && apt-get install -y python3-dev python3-pip +RUN pip3 install setuptools wheel tox posix_ipc numpy + +# Setup user +USER ue4 + +WORKDIR /home/ue4/source/holoocean/ + +# This should be COPY ../ but docker doesn't allow copying files outside the context +# To copy the project files either run the build command in this directory with the +# previous directory as the context: docker build -t frostlab/holoocean[:tag] -f ./Dockerfile .. +# or run it from the parent directory and provide the docker file location +# docker build -t frostlab/holoocean[:tag] -f ./docker/Dockerfile . +COPY --chown=ue4 ./ . +RUN pip3 install . + +CMD ["/bin/bash"] \ No newline at end of file diff --git a/Util/Docker/holoocean/Dockerfile_latest b/Util/Docker/holoocean/Dockerfile_latest new file mode 100644 index 0000000000..bc55373e45 --- /dev/null +++ b/Util/Docker/holoocean/Dockerfile_latest @@ -0,0 +1,5 @@ +FROM frostlab/holoocean:base + +RUN python3 -c 'import holoocean; holoocean.install("Ocean"); holoocean.install("TestWorlds")' + +CMD ["/bin/bash"] \ No newline at end of file diff --git a/Util/Docker/holoocean/Dockerfile_ocean b/Util/Docker/holoocean/Dockerfile_ocean new file mode 100644 index 0000000000..6527b0c5a5 --- /dev/null +++ b/Util/Docker/holoocean/Dockerfile_ocean @@ -0,0 +1,5 @@ +FROM frostlab/holoocean:base + +RUN python3 -c 'import holoocean; holoocean.install("Ocean")' + +CMD ["/bin/bash"] \ No newline at end of file