diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 321d159a7..50a89a7ef 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -168,6 +168,8 @@ namespace airlib bool isRecording(); void simSetWind(const Vector3r& wind) const; + void simSetExtForce(const Vector3r& ext_force) const; + vector listVehicles(); std::string getSettingsString() const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index cec9feef5..741c0116d 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -87,6 +87,7 @@ namespace airlib virtual bool isRecording() const = 0; virtual void setWind(const Vector3r& wind) const = 0; + virtual void setExtForce(const Vector3r& ext_force) const = 0; virtual vector listVehicles() const = 0; virtual std::string getSettingsString() const = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 2d839501f..8670879a5 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -482,12 +482,13 @@ namespace airlib CameraDirectorSetting camera_director; std::map> beacons; std::map> passive_echo_beacons; - float speed_unit_factor = 1.0f; + float speed_unit_factor = 1.0f; std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; Vector3r wind = Vector3r::Zero(); - std::string settings_text_ = ""; + Vector3r ext_force = Vector3r::Zero(); std::string material_list_file = ""; + std::string settings_text_ = ""; public: //methods static AirSimSettings& singleton() @@ -543,7 +544,7 @@ namespace airlib Settings& settings_json = Settings::singleton(); //write some settings_json in new file otherwise the string "null" is written if all settings_json are empty settings_json.setString("SeeDocsAt", "https://cosysgit.uantwerpen.be/sensorsimulation/airsim/-/blob/master/docs/settings.md"); - settings_json.setDouble("SettingsVersion", 1.2); + settings_json.setDouble("SettingsVersion", 2.0); std::string settings_filename = Settings::getUserDirectoryFullPath("settings.json"); //TODO: there is a crash in Linux due to settings_json.saveJSonString(). Remove this workaround after we only support Unreal 4.17 @@ -802,7 +803,6 @@ namespace airlib capture_settings[i] = CaptureSetting(); } capture_settings.at(Utils::toNumeric(ImageType::Scene)).target_gamma = CaptureSetting::kSceneTargetGamma; - capture_settings.at(Utils::toNumeric(ImageType::Segmentation)).target_gamma = 1; } static void loadCaptureSettings(const Settings& settings_json, CaptureSettingsMap& capture_settings) @@ -1407,6 +1407,13 @@ namespace airlib wind = createVectorSetting(child_json, wind); } } + { + // External Force Settings + Settings child_json; + if (settings_json.getChild("ExternalForce", child_json)) { + ext_force = createVectorSetting(child_json, ext_force); + } + } } static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults) @@ -1416,7 +1423,6 @@ namespace airlib camera_defaults = createCameraSetting(child_json, camera_defaults); } } - static void loadCameraDirectorSetting(const Settings& settings_json, CameraDirectorSetting& camera_director, const std::string& simmode_name) { diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index f403ed84c..c2d4c6cc8 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/AirLib/include/physics/FastPhysicsEngine.hpp @@ -22,8 +22,8 @@ namespace airlib class FastPhysicsEngine : public PhysicsEngineBase { public: - FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero()) - : enable_ground_lock_(enable_ground_lock), wind_(wind) + FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero(), Vector3r ext_force = Vector3r::Zero()) + : enable_ground_lock_(enable_ground_lock), wind_(wind), ext_force_(ext_force) { setName("FastPhysicsEngine"); } @@ -69,6 +69,11 @@ namespace airlib { wind_ = wind; } + // Set External Force + void setExtForce(const Vector3r& ext_force) override + { + ext_force_ = ext_force; + } private: void initPhysicsBody(PhysicsBody* body_ptr) @@ -88,7 +93,7 @@ namespace airlib //first compute the response as if there was no collision //this is necessary to take in to account forces and torques generated by body - getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_); + getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_, ext_force_); //if there is collision, see if we need collision response const CollisionInfo collision_info = body.getCollisionInfo(); @@ -261,8 +266,11 @@ namespace airlib } } - static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation, - const Vector3r& linear_vel, const Vector3r& angular_vel_body, const Vector3r& wind_world) + static Wrench getDragWrench(const PhysicsBody& body, + const Quaternionr& orientation, + const Vector3r& linear_vel, + const Vector3r& angular_vel_body, + const Vector3r& wind_world) { //add linear drag due to velocity we had since last dt seconds + wind //drag vector magnitude is proportional to v^2, direction opposite of velocity @@ -323,7 +331,7 @@ namespace airlib } static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current, - Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind) + Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind, const Vector3r& ext_force) { const real_T dt_real = static_cast(dt); @@ -356,7 +364,11 @@ namespace airlib avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real); const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular, wind); - next_wrench = body_wrench + drag_wrench; + // ext_force is defined in world space + Wrench ext_force_wrench = Wrench::zero(); + ext_force_wrench.force = ext_force; + + next_wrench = body_wrench + drag_wrench + ext_force_wrench; //Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str())); //Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str())); @@ -459,6 +471,7 @@ namespace airlib bool enable_ground_lock_; TTimePoint last_message_time; Vector3r wind_; + Vector3r ext_force_; }; } } //namespace diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp index 98fe72e20..64b1db2f1 100644 --- a/AirLib/include/physics/PhysicsEngineBase.hpp +++ b/AirLib/include/physics/PhysicsEngineBase.hpp @@ -28,6 +28,7 @@ namespace airlib } virtual void setWind(const Vector3r& wind) { unused(wind); }; + virtual void setExtForce(const Vector3r& ext_force) { unused(ext_force); }; }; } } //namespace diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 16d9b3eda..b5fcad4c1 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -655,7 +655,11 @@ __pragma(warning(disable : 4239)) RpcLibAdaptorsBase::Vector3r conv_wind(wind); pimpl_->client.call("simSetWind", conv_wind); } - + void RpcLibClientBase::simSetExtForce(const Vector3r& ext_force) const + { + RpcLibAdaptorsBase::Vector3r conv_ext_force(ext_force); + pimpl_->client.call("simSetExtForce", conv_ext_force); + } vector RpcLibClientBase::listVehicles() { return pimpl_->client.call("listVehicles").as>(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 5d88d5230..c7b973ba3 100755 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -278,7 +278,6 @@ namespace airlib const auto& response = getWorldSimApi()->getDetections(type, CameraDetails(camera_name, vehicle_name)); return RpcLibAdaptorsBase::DetectionInfo::from(response); }); - pimpl_->server.bind("reset", [&]() -> void { //Exit if already resetting. static bool resetInProgress; @@ -545,6 +544,10 @@ namespace airlib getWorldSimApi()->setWind(wind.to()); }); + pimpl_->server.bind("simSetExtForce", [&](const RpcLibAdaptorsBase::Vector3r& ext_force) -> void { + getWorldSimApi()->setExtForce(ext_force.to()); + }); + pimpl_->server.bind("listVehicles", [&]() -> vector { return getWorldSimApi()->listVehicles(); }); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 652d3e650..59c1d8fd9 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -159,7 +159,7 @@ __pragma(warning(disable : 4239)) MultirotorRpcLibClient* MultirotorRpcLibClient::moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { - pimpl_->last_future = static_cast(getClient())->async_call("movetoGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); + pimpl_->last_future = static_cast(getClient())->async_call("moveToGPS", latitude, longitude, altitude, velocity, timeout_sec, drivetrain, MultirotorRpcLibAdaptors::YawMode(yaw_mode), lookahead, adaptive_lookahead, vehicle_name); return this; } diff --git a/HelloDrone/main.cpp b/HelloDrone/main.cpp index a6c8a1446..ec94222b3 100644 --- a/HelloDrone/main.cpp +++ b/HelloDrone/main.cpp @@ -33,7 +33,7 @@ int main() const std::vector& response = client.simGetImages(request); std::cout << "# of images received: " << response.size() << std::endl; - if (!response.size()) { + if (response.size()) { std::cout << "Enter path with ending separator to save images (leave empty for no save)" << std::endl; std::string path; std::getline(std::cin, path); diff --git a/LICENSE b/LICENSE index 75fca5ef2..3b7ed096d 100644 --- a/LICENSE +++ b/LICENSE @@ -1,9 +1,10 @@ +============================= Original License ================================ The MIT License (MIT) MSR Aerial Informatics and Robotics Platform MSR Aerial Informatics and Robotics Simulator (AirSim) -Copyright (c) Microsoft Corporation +Copyright (c) Microsoft Corporation. 2022 All rights reserved. @@ -12,3 +13,36 @@ MIT License Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the ""Software""), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. +=============================================================================== +===================== New License by Codex Laboratories ====================== +The MIT License (MIT) + +MSR Aerial Informatics and Robotics Platform +MSR Aerial Informatics and Robotics Simulator (AirSim) + +Copyright (c) Codex Laboratories LLC. 2022 + +All rights reserved. + +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the ""Software""), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE + +=============================================================================== +======================== New License by Cosys-Lab ============================ +The MIT License (MIT) + +MSR Aerial Informatics and Robotics Platform +MSR Aerial Informatics and Robotics Simulator (AirSim) + +Copyright (c) University of Antwerp, Belgium 2024 + +All rights reserved. + +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the ""Software""), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. +THE SOFTWARE IS PROVIDED *AS IS*, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE \ No newline at end of file diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index d1c9141dd..83ee1bb7b 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -40,14 +40,44 @@ def getMinRequiredServerVersion(self): def getMinRequiredClientVersion(self): return self.client.call('getMinRequiredClientVersion') - # basic flight control +#basic flight control def enableApiControl(self, is_enabled, vehicle_name = ''): - return self.client.call('enableApiControl', is_enabled, vehicle_name) + """ + Enables or disables API control for vehicle corresponding to vehicle_name + + Args: + is_enabled (bool): True to enable, False to disable API control + vehicle_name (str, optional): Name of the vehicle to send this command to + """ + self.client.call('enableApiControl', is_enabled, vehicle_name) + def isApiControlEnabled(self, vehicle_name = ''): + """ + Returns true if API control is established. + + If false (which is default) then API calls would be ignored. After a successful call to `enableApiControl`, `isApiControlEnabled` should return true. + + Args: + vehicle_name (str, optional): Name of the vehicle + + Returns: + bool: If API control is enabled + """ return self.client.call('isApiControlEnabled', vehicle_name) + def armDisarm(self, arm, vehicle_name = ''): + """ + Arms or disarms vehicle + + Args: + arm (bool): True to arm, False to disarm the vehicle + vehicle_name (str, optional): Name of the vehicle to send this command to + + Returns: + bool: Success + """ return self.client.call('armDisarm', arm, vehicle_name) - + def simPause(self, is_paused): """ Pauses simulation @@ -75,20 +105,35 @@ def simContinueForTime(self, seconds): """ self.client.call('simContinueForTime', seconds) + def simContinueForFrames(self, frames): + """ + Continue (or resume if paused) the simulation for the specified number of frames, after which the simulation will be paused. + + Args: + frames (int): Frames to run the simulation for + """ + self.client.call('simContinueForFrames', frames) + def getHomeGeoPoint(self, vehicle_name = ''): + """ + Get the Home location of the vehicle + + Args: + vehicle_name (str, optional): Name of vehicle to get home location of + + Returns: + GeoPoint: Home location of the vehicle + """ return GeoPoint.from_msgpack(self.client.call('getHomeGeoPoint', vehicle_name)) - def confirmConnection(self, name=''): + def confirmConnection(self): """ Checks state of connection every 1 sec and reports it in Console so user can see the progress for connection. """ if self.ping(): - if name == '': - print("Connected to AirSim API!") - else: - print(name + " connected to AirSim API!") + print("Connected!") else: - print("Ping returned false!") + print("Ping returned false!") server_ver = self.getServerVersion() client_ver = self.getClientVersion() server_min_ver = self.getMinRequiredServerVersion() @@ -409,7 +454,7 @@ def simSetVehiclePose(self, pose, ignore_collision, vehicle_name = ''): def simGetVehiclePose(self, vehicle_name = ''): """ The position inside the returned Pose is in the frame of the vehicle's starting point - + Args: vehicle_name (str, optional): Name of the vehicle to get the Pose of @@ -419,8 +464,30 @@ def simGetVehiclePose(self, vehicle_name = ''): pose = self.client.call('simGetVehiclePose', vehicle_name) return Pose.from_msgpack(pose) + def simSetTraceLine(self, color_rgba, thickness=1.0, vehicle_name = ''): + """ + Modify the color and thickness of the line when Tracing is enabled + + Tracing can be enabled by pressing T in the Editor or setting `EnableTrace` to `True` in the Vehicle Settings + + Args: + color_rgba (list): desired RGBA values from 0.0 to 1.0 + thickness (float, optional): Thickness of the line + vehicle_name (string, optional): Name of the vehicle to set Trace line values for + """ + self.client.call('simSetTraceLine', color_rgba, thickness, vehicle_name) + def simGetObjectPose(self, object_name, ned=True): - pose = self.client.call('simGetObjectPose', object_name, ned) + """ + The position inside the returned Pose is in the world frame + + Args: + object_name (str): Object to get the Pose of + + Returns: + Pose: + """ + pose = self.client.call('simGetObjectPose', object_name, ned=True) return Pose.from_msgpack(pose) def simSetObjectPose(self, object_name, pose, teleport = True): @@ -481,6 +548,53 @@ def simListSceneObjects(self, name_regex = '.*'): """ return self.client.call('simListSceneObjects', name_regex) + def simLoadLevel(self, level_name): + """ + Loads a level specified by its name + + Args: + level_name (str): Name of the level to load + + Returns: + bool: True if the level was successfully loaded + """ + return self.client.call('simLoadLevel', level_name) + + def simListAssets(self): + """ + Lists all the assets present in the Asset Registry + + Returns: + list[str]: Names of all the assets + """ + return self.client.call('simListAssets') + + def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=False, is_blueprint=False): + """Spawned selected object in the world + + Args: + object_name (str): Desired name of new object + asset_name (str): Name of asset(mesh) in the project database + pose (airsim.Pose): Desired pose of object + scale (airsim.Vector3r): Desired scale of object + physics_enabled (bool, optional): Whether to enable physics for the object + is_blueprint (bool, optional): Whether to spawn a blueprint or an actor + + Returns: + str: Name of spawned object, in case it had to be modified + """ + return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled, is_blueprint) + + def simDestroyObject(self, object_name): + """Removes selected object from the world + + Args: + object_name (str): Name of object to be removed + + Returns: + bool: True if object is queued up for removal + """ + return self.client.call('simDestroyObject', object_name) def simListInstanceSegmentationObjects(self): return self.client.call('simListInstanceSegmentationObjects') @@ -556,54 +670,6 @@ def simGetDetections(self, camera_name, image_type, vehicle_name = '', external responses_raw = self.client.call('simGetDetections', camera_name, image_type, vehicle_name, external) return [DetectionInfo.from_msgpack(response_raw) for response_raw in responses_raw] - def simLoadLevel(self, level_name): - """ - Loads a level specified by its name - - Args: - level_name (str): Name of the level to load - - Returns: - bool: True if the level was successfully loaded - """ - return self.client.call('simLoadLevel', level_name) - - def simListAssets(self): - """ - Lists all the assets present in the Asset Registry - - Returns: - list[str]: Names of all the assets - """ - return self.client.call('simListAssets') - - def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=False, is_blueprint=False): - """Spawned selected object in the world - - Args: - object_name (str): Desired name of new object - asset_name (str): Name of asset(mesh) in the project database - pose (airsim.Pose): Desired pose of object - scale (airsim.Vector3r): Desired scale of object - physics_enabled (bool, optional): Whether to enable physics for the object - is_blueprint (bool, optional): Whether to spawn a blueprint or an actor - - Returns: - str: Name of spawned object, in case it had to be modified - """ - return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled, is_blueprint) - - def simDestroyObject(self, object_name): - """Removes selected object from the world - - Args: - object_name (str): Name of object to be removed - - Returns: - bool: True if object is queued up for removal - """ - return self.client.call('simDestroyObject', object_name) - def simPrintLogMessage(self, message, message_param = "", severity = 0): """ Prints the specified message in the simulator's window. @@ -890,7 +956,150 @@ def simPlotArrows(self, points_start, points_end, color_rgba=[1.0, 0.0, 0.0, 1.0 """ self.client.call('simPlotArrows', points_start, points_end, color_rgba, thickness, arrow_size, duration, is_persistent) -#----------------------------------- Multirotor APIs --------------------------------------------- + + def simPlotStrings(self, strings, positions, scale = 5, color_rgba=[1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of strings at desired positions in World NED frame. + + Args: + strings (list[String], optional): List of strings to plot + positions (list[Vector3r]): List of positions where the strings should be plotted. Should be in one-to-one correspondence with the strings' list + scale (float, optional): Font scale of transform name + color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotStrings', strings, positions, scale, color_rgba, duration) + + def simPlotTransforms(self, poses, scale = 5.0, thickness = 5.0, duration = -1.0, is_persistent = False): + """ + Plots a list of transforms in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + scale (float, optional): Length of transforms' axes + thickness (float, optional): Thickness of transforms' axes + duration (float, optional): Duration (seconds) to plot for + is_persistent (bool, optional): If set to True, the desired object will be plotted for infinite time. + """ + self.client.call('simPlotTransforms', poses, scale, thickness, duration, is_persistent) + + def simPlotTransformsWithNames(self, poses, names, tf_scale = 5.0, tf_thickness = 5.0, text_scale = 10.0, text_color_rgba = [1.0, 0.0, 0.0, 1.0], duration = -1.0): + """ + Plots a list of transforms with their names in World NED frame. + + Args: + poses (list[Pose]): List of Pose objects representing the transforms to plot + names (list[string]): List of strings with one-to-one correspondence to list of poses + tf_scale (float, optional): Length of transforms' axes + tf_thickness (float, optional): Thickness of transforms' axes + text_scale (float, optional): Font scale of transform name + text_color_rgba (list, optional): desired RGBA values from 0.0 to 1.0 for the transform name + duration (float, optional): Duration (seconds) to plot for + """ + self.client.call('simPlotTransformsWithNames', poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration) + + def cancelLastTask(self, vehicle_name = ''): + """ + Cancel previous Async task + + Args: + vehicle_name (str, optional): Name of the vehicle + """ + self.client.call('cancelLastTask', vehicle_name) + +#Recording APIs + def startRecording(self): + """ + Start Recording + + Recording will be done according to the settings + """ + self.client.call('startRecording') + + def stopRecording(self): + """ + Stop Recording + """ + self.client.call('stopRecording') + + def isRecording(self): + """ + Whether Recording is running or not + + Returns: + bool: True if Recording, else False + """ + return self.client.call('isRecording') + + def simSetWind(self, wind): + """ + Set simulated wind, in World frame, NED direction, m/s + + Args: + wind (Vector3r): Wind, in World frame, NED direction, in m/s + """ + self.client.call('simSetWind', wind) + + def simCreateVoxelGrid(self, position, x, y, z, res, of): + """ + Construct and save a binvox-formatted voxel grid of environment + + Args: + position (Vector3r): Position around which voxel grid is centered in m + x, y, z (int): Size of each voxel grid dimension in m + res (float): Resolution of voxel grid in m + of (str): Name of output file to save voxel grid as + + Returns: + bool: True if output written to file successfully, else False + """ + return self.client.call('simCreateVoxelGrid', position, x, y, z, res, of) + +#Add new vehicle via RPC + def simAddVehicle(self, vehicle_name, vehicle_type, pose, pawn_path = ""): + """ + Create vehicle at runtime + + Args: + vehicle_name (str): Name of the vehicle being created + vehicle_type (str): Type of vehicle, e.g. "simpleflight" + pose (Pose): Initial pose of the vehicle + pawn_path (str, optional): Vehicle blueprint path, default empty wbich uses the default blueprint for the vehicle type + + Returns: + bool: Whether vehicle was created + """ + return self.client.call('simAddVehicle', vehicle_name, vehicle_type, pose, pawn_path) + + def listVehicles(self): + """ + Lists the names of current vehicles + + Returns: + list[str]: List containing names of all vehicles + """ + return self.client.call('listVehicles') + + def getSettingsString(self): + """ + Fetch the settings text being used by AirSim + + Returns: + str: Settings text in JSON format + """ + return self.client.call('getSettingsString') + + def simSetExtForce(self, ext_force): + """ + Set arbitrary external forces, in World frame, NED direction. Can be used + for implementing simple payloads. + + Args: + ext_force (Vector3r): Force, in World frame, NED direction, in N + """ + self.client.call('simSetExtForce', ext_force) + +# ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): super(MultirotorClient, self).__init__(ip, port, timeout_value) @@ -1395,4 +1604,4 @@ def getCarControls(self, vehicle_name=''): CarControls: """ controls_raw = self.client.call('getCarControls', vehicle_name) - return CarControls.from_msgpack(controls_raw) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) diff --git a/README.md b/README.md index fa5ea3db2..c9d2ef97d 100755 --- a/README.md +++ b/README.md @@ -74,7 +74,7 @@ A manually maintained fork of this repository is available to the public: https: * Added BoxCar vehicle model to the Car SimMode to have a smaller vehicle to use in indoor spaces. * Updated standard camera render resolution target to 960x540. Updated standard uncompressed image format to RGB instead of BGR (this breaks OpenCV support but fixes ROS images). * Added option to Cameras, EchoSensor and GPULidar to ignore certain objects with the _MarkedIgnore_ Unreal tag and enabling the "IgnoreMarked" setting in [the settings file](docs/settings.md). -* Updated Unreal to 4.27 (custom fork: [https://github.com/WouterJansen/UnrealEngine/tree/4.27-cosys](https://github.com/WouterJansen/UnrealEngine/tree/4.27-cosys)) +* Updated Unreal to 5.2.1 by merging changes of [Colosseum by Codex Laboratories LLC](https://github.com/CodexLabsLLC/Colosseum). * Dropped support for Unity Environments. Some more details on our changes can be found in the [changelog](CHANGELOG.md). diff --git a/Unreal/Plugins/AirSim/AirSim.uplugin b/Unreal/Plugins/AirSim/AirSim.uplugin index 76bd54b49..abd639cef 100644 --- a/Unreal/Plugins/AirSim/AirSim.uplugin +++ b/Unreal/Plugins/AirSim/AirSim.uplugin @@ -22,9 +22,9 @@ } ], "Plugins": [ - { - "Name": "PhysXVehicles", - "Enabled": true - } + { + "Name": "ChaosVehiclesPlugin", + "Enabled": true + } ] } diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index 32798f93d..6f43714bc 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -25,10 +25,12 @@ #include #include "common/common_utils/Utils.hpp" #include "Modules/ModuleManager.h" -#include "ARFilter.h" -#include "AssetRegistryModule.h" +#include "AssetRegistry/ARFilter.h" +#include "AssetRegistry/AssetRegistryModule.h" #include "DetectionComponent.h" #include "Components/LineBatchComponent.h" +#include "CineCameraComponent.h" + /* //TODO: change naming conventions to same as other files? Naming conventions in this file: @@ -389,8 +391,8 @@ void UAirBlueprintLib::GenerateAssetRegistryMap(const UObject* context, TMapGetFName()); - Filter.ClassNames.Add(UBlueprint::StaticClass()->GetFName()); + Filter.ClassPaths.Add(UStaticMesh::StaticClass()->GetClassPathName()); + Filter.ClassPaths.Add(UBlueprint::StaticClass()->GetClassPathName()); Filter.bRecursivePaths = true; auto world = context->GetWorld(); @@ -730,9 +732,9 @@ std::vector UAirBlueprintLib::Ge ENQUEUE_RENDER_COMMAND(GetVertexBuffer) ( [vertex_buffer, data](FRHICommandListImmediate& RHICmdList) { - FVector* indices = (FVector*)RHILockVertexBuffer(vertex_buffer->VertexBufferRHI, 0, vertex_buffer->VertexBufferRHI->GetSize(), RLM_ReadOnly); + FVector* indices = (FVector*)RHILockBuffer(vertex_buffer->VertexBufferRHI, 0, vertex_buffer->VertexBufferRHI->GetSize(), RLM_ReadOnly); memcpy(data, indices, vertex_buffer->VertexBufferRHI->GetSize()); - RHIUnlockVertexBuffer(vertex_buffer->VertexBufferRHI); + RHIUnlockBuffer(vertex_buffer->VertexBufferRHI); }); #if ((ENGINE_MAJOR_VERSION > 4) || (ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION >= 27)) @@ -752,9 +754,9 @@ std::vector UAirBlueprintLib::Ge ENQUEUE_RENDER_COMMAND(GetIndexBuffer) ( [IndexBuffer, data_ptr](FRHICommandListImmediate& RHICmdList) { - uint16_t* indices = (uint16_t*)RHILockIndexBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); + uint16_t* indices = (uint16_t*)RHILockBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); memcpy(data_ptr, indices, IndexBuffer->IndexBufferRHI->GetSize()); - RHIUnlockIndexBuffer(IndexBuffer->IndexBufferRHI); + RHIUnlockBuffer(IndexBuffer->IndexBufferRHI); }); //Need to force the render command to go through cause on the next iteration the buffer no longer exists @@ -775,9 +777,9 @@ std::vector UAirBlueprintLib::Ge ENQUEUE_RENDER_COMMAND(GetIndexBuffer) ( [IndexBuffer, data_ptr](FRHICommandListImmediate& RHICmdList) { - uint32_t* indices = (uint32_t*)RHILockIndexBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); + uint32_t* indices = (uint32_t*)RHILockBuffer(IndexBuffer->IndexBufferRHI, 0, IndexBuffer->IndexBufferRHI->GetSize(), RLM_ReadOnly); memcpy(data_ptr, indices, IndexBuffer->IndexBufferRHI->GetSize()); - RHIUnlockIndexBuffer(IndexBuffer->IndexBufferRHI); + RHIUnlockBuffer(IndexBuffer->IndexBufferRHI); }); FlushRenderingCommands(); diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 30cc6727e..6afcd750f 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -4,7 +4,7 @@ #pragma once #include "CoreMinimal.h" -#include "Runtime/AssetRegistry/Public/AssetRegistryModule.h" +#include "AssetRegistry/AssetRegistryModule.h" #include "GameFramework/Actor.h" #include "Components/InputComponent.h" #include "EngineUtils.h" diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 87ffa10b1..9663dd10b 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -78,8 +78,8 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "PhysX", "PhysXVehicles", "PhysXVehicleLib", "APEX", "Landscape", "CinematicCamera" }); - PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "PhysX", "PhysXVehicles", "PhysXVehicleLib" }); + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry", "PhysicsCore", "ChaosVehicles", "Landscape", "CinematicCamera" }); + PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore", "RenderCore", "ChaosVehicles" }); //suppress VC++ proprietary warnings PublicDefinitions.Add("_SCL_SECURE_NO_WARNINGS=1"); @@ -156,4 +156,4 @@ private bool AddLibDependency(string LibName, string LibPath, string LibFileName return isLibrarySupported; } -} \ No newline at end of file +} diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 61256053b..e93679b40 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -866,7 +866,9 @@ void APIPCamera::setFocusAperture(float focus_aperture) void APIPCamera::enableFocusPlane(bool enable) { +#if WITH_EDITOR camera_->FocusSettings.bDrawDebugFocusPlane = enable; +#endif } std::string APIPCamera::getCurrentFieldOfView() const diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 0b9f28952..0eea6461d 100755 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -231,7 +231,7 @@ msr::airlib::RCData PawnSimApi::getRCData() const void PawnSimApi::displayCollisionEffect(FVector hit_location, const FHitResult& hit) { - if (params_.collision_display_template != nullptr && Utils::isDefinitelyLessThan(hit.ImpactNormal.Z, 0.0f)) { + if (params_.collision_display_template != nullptr && Utils::isDefinitelyLessThan(hit.ImpactNormal.Z, 0.0f)) { UParticleSystemComponent* particles = UGameplayStatics::SpawnEmitterAtLocation(params_.pawn->GetWorld(), params_.collision_display_template, FTransform(hit_location), diff --git a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp index 533ffcca3..4fc06cc38 100644 --- a/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp +++ b/Unreal/Plugins/AirSim/Source/Recording/RecordingFile.cpp @@ -1,5 +1,5 @@ #include "RecordingFile.h" -#include "HAL/PlatformFilemanager.h" +#include "HAL/PlatformFileManager.h" #include "Misc/FileHelper.h" #include #include "ImageUtils.h" diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 15ccae1be..8d20db6c1 100755 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -98,7 +98,6 @@ void ASimModeBase::toggleLoadingScreen(bool is_visible) void ASimModeBase::BeginPlay() { - Super::BeginPlay(); debug_reporter_.initialize(false); @@ -345,10 +344,10 @@ void ASimModeBase::initializeTimeOfDay() static const FName sun_prop_name(TEXT("Directional light actor")); auto* p = sky_sphere_class_->FindPropertyByName(sun_prop_name); -#if ENGINE_MINOR_VERSION > 24 +#if ((ENGINE_MAJOR_VERSION > 4) || (ENGINE_MAJOR_VERSION == 4 && ENGINE_MINOR_VERSION >= 27)) FObjectProperty* sun_prop = CastFieldChecked(p); #else - UObjectProperty* sun_prop = Cast(p); + FObjectProperty* sun_prop = Cast(p); #endif UObject* sun_obj = sun_prop->GetObjectPropertyValue_InContainer(sky_sphere_); @@ -431,6 +430,13 @@ void ASimModeBase::setWind(const msr::airlib::Vector3r& wind) const throw std::domain_error("setWind not implemented by SimMode"); } +void ASimModeBase::setExtForce(const msr::airlib::Vector3r& ext_force) const +{ + // should be overridden by derived class + unused(ext_force); + throw std::domain_error("setExtForce not implemented by SimMode"); +} + std::unique_ptr ASimModeBase::createApiServer() const { //this will be the case when compilation with RPCLIB is disabled or simmode doesn't support APIs @@ -471,7 +477,7 @@ void ASimModeBase::Tick(float DeltaSeconds) updateDebugReport(debug_reporter_); - //drawLidarDebugPoints(); + drawLidarDebugPoints(); drawDistanceSensorDebugPoints(); @@ -580,6 +586,33 @@ void ASimModeBase::initializeCameraDirector(const FTransform& camera_transform, } } +void ASimModeBase::initializeExternalCameras() +{ + FActorSpawnParameters camera_spawn_params; + camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; + const auto& transform = getGlobalNedTransform(); + + //for each camera in settings + for (const auto& camera_setting_pair : getSettings().external_cameras) { + const auto& setting = camera_setting_pair.second; + + //get pose + FVector position = transform.fromLocalNed(setting.position) - transform.fromLocalNed(Vector3r::Zero()); + FTransform camera_transform(FRotator(setting.rotation.pitch, setting.rotation.yaw, setting.rotation.roll), + position, + FVector(1., 1., 1.)); + + //spawn and attach camera to pawn + camera_spawn_params.Name = FName(("external_" + camera_setting_pair.first).c_str()); + APIPCamera* camera = this->GetWorld()->SpawnActor(pip_camera_class, camera_transform, camera_spawn_params); + + camera->setupCameraFromSettings(setting, transform); + + //add on to our collection + external_cameras_.insert_or_assign(camera_setting_pair.first, camera); + } +} + bool ASimModeBase::toggleRecording() { if (isRecording()) @@ -615,12 +648,13 @@ void ASimModeBase::toggleTraceAll() const APIPCamera* ASimModeBase::getCamera(const msr::airlib::CameraDetails& camera_details) const { - return getVehicleSimApi(camera_details.vehicle_name)->getCamera(camera_details.camera_name); + return camera_details.external ? getExternalCamera(camera_details.camera_name) + : getVehicleSimApi(camera_details.vehicle_name)->getCamera(camera_details.camera_name); } -const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name) const +const UnrealImageCapture* ASimModeBase::getImageCapture(const std::string& vehicle_name, bool external) const { - return getVehicleSimApi(vehicle_name)->getImageCapture(); + return external ? external_image_capture_.get() : getVehicleSimApi(vehicle_name)->getImageCapture(); } //API server start/stop @@ -807,111 +841,6 @@ void ASimModeBase::setupVehiclesAndCamera() } } } - - //add beacons from settings - for (auto const& beacon_setting_pair : getSettings().beacons) - { - //if vehicle is of type for derived SimMode and auto creatable - const auto& beacon_setting = *beacon_setting_pair.second; - //if (beacon_setting.auto_create && - //isVehicleTypeSupported(beacon_setting.beacon_type)) { - if (beacon_setting.auto_create) { - //compute initial pose - FVector spawn_position = uu_origin.GetLocation(); - msr::airlib::Vector3r settings_position = beacon_setting.position; - /*FVector globalOffset = getGlobalNedTransform().getGlobalOffset(); - - settings_position.x() += globalOffset.X; - settings_position.y() += globalOffset.Y; - settings_position.z() += globalOffset.Z;*/ - - - if (!msr::airlib::VectorMath::hasNan(settings_position)) - spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); - FRotator spawn_rotation = toFRotator(beacon_setting.rotation, uu_origin.Rotator()); - - //spawn beacon pawn - FActorSpawnParameters pawn_spawn_params; - FString comboName = beacon_setting.beacon_name.c_str() + FString(":") + beacon_setting.beacon_pawn_name.c_str(); - pawn_spawn_params.Name = FName(*comboName); - pawn_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - - //auto beacon_bp_class = UAirBlueprintLib::LoadClass(getSettings().pawn_paths.at(beacon_setting.beacon_pawn_name).pawn_bp); - //FActorSpawnParameters SpawnInfo; - // TODO: Make the child sim modes responsible for casting the types. - //ATemplateBeacon* spawned_beacon = static_cast(this->GetWorld()->SpawnActor(beacon_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params2)); - - if (beacon_setting.beacon_type.compare("fiducialmarker") == 0) { - AFiducialBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - spawned_beacon->setScale(beacon_setting.scale); - } - else if (beacon_setting.beacon_type.compare("uwbbeacon") == 0) { - AUWBBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - } - else if (beacon_setting.beacon_type.compare("wifibeacon") == 0) { - AWifiBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - } - else if (beacon_setting.beacon_type.compare("dynamicblockbeacon") == 0) { - ADynamicBlockBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - } - else if (beacon_setting.beacon_type.compare("dynamicrackbeacon") == 0) { - ADynamicRackBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - } - else { - ATemplateBeacon* spawned_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, pawn_spawn_params)); - } - - //this->GetWorld()->SpawnActor(ATemplateBeacon) - /*AActor* spawned_actor = static_cast(this->GetWorld()->SpawnActor( - beacon_bp_class, &spawn_position, &spawn_rotation, pawn_spawn_params2)); - - AirsimVehicle* spawned_pawn2 = dynamic_cast(spawned_actor); - - spawned_actors_.Add(spawned_pawn2->GetPawn()); - pawns.Add(spawned_pawn2); - - if (beacon_setting.is_fpv_vehicle) - fpv_pawn = spawned_pawn2->GetPawn();*/ - } - } - - //add passive echo beacons from settings - for (auto const& passive_echo_beacon_setting_pair : getSettings().passive_echo_beacons) - { - const auto& passive_echo_beacon_setting = *passive_echo_beacon_setting_pair.second; - //compute initial pose - FVector spawn_position = FVector(0, 0, 0); - msr::airlib::Vector3r settings_position = passive_echo_beacon_setting.position; - if (!msr::airlib::VectorMath::hasNan(settings_position)) - spawn_position = global_ned_transform_->toFVector(settings_position, 100, true); - FRotator spawn_rotation = toFRotator(passive_echo_beacon_setting.rotation, FRotator()); - - //spawn passive echo beacon actor - FActorSpawnParameters actor_spawn_params; - actor_spawn_params.Name = FName(passive_echo_beacon_setting.name.c_str()); - actor_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - actor_spawn_params.bDeferConstruction = true; - APassiveEchoBeacon* spawned_passive_echo_beacon = static_cast(GetWorld()->SpawnActor(spawn_position, spawn_rotation, actor_spawn_params)); - spawned_passive_echo_beacon->enable_ = passive_echo_beacon_setting.enable; - spawned_passive_echo_beacon->initial_directions_ = passive_echo_beacon_setting.initial_directions; - spawned_passive_echo_beacon->initial_lower_azimuth_limit_ = passive_echo_beacon_setting.initial_lower_azimuth_limit; - spawned_passive_echo_beacon->initial_upper_azimuth_limit_ = passive_echo_beacon_setting.initial_upper_azimuth_limit; - spawned_passive_echo_beacon->initial_lower_elevation_limit_ = passive_echo_beacon_setting.initial_lower_elevation_limit; - spawned_passive_echo_beacon->initial_upper_elevation_limit_ = passive_echo_beacon_setting.initial_upper_elevation_limit; - spawned_passive_echo_beacon->attenuation_limit_ = passive_echo_beacon_setting.attenuation_limit; - spawned_passive_echo_beacon->reflection_distance_limit_ = passive_echo_beacon_setting.reflection_distance_limit; - spawned_passive_echo_beacon->reflection_only_final_ = passive_echo_beacon_setting.reflection_only_final; - spawned_passive_echo_beacon->attenuation_per_distance_ = passive_echo_beacon_setting.attenuation_per_distance; - spawned_passive_echo_beacon->attenuation_per_reflection_ = passive_echo_beacon_setting.attenuation_per_reflection; - spawned_passive_echo_beacon->distance_limit_ = passive_echo_beacon_setting.distance_limit; - spawned_passive_echo_beacon->reflection_limit_ = passive_echo_beacon_setting.reflection_limit; - spawned_passive_echo_beacon->draw_debug_location_ = passive_echo_beacon_setting.draw_debug_location; - spawned_passive_echo_beacon->draw_debug_all_points_ = passive_echo_beacon_setting.draw_debug_all_points; - spawned_passive_echo_beacon->draw_debug_all_lines_ = passive_echo_beacon_setting.draw_debug_all_lines; - spawned_passive_echo_beacon->draw_debug_duration_ = passive_echo_beacon_setting.draw_debug_duration; - spawned_passive_echo_beacon->FinishSpawning(FTransform(spawn_rotation, spawn_position)); - } - //create API objects for each pawn we have for (AActor* pawn : pawns) { auto vehicle_pawn = static_cast(pawn); @@ -926,6 +855,10 @@ void ASimModeBase::setupVehiclesAndCamera() } } + // Create External Cameras + initializeExternalCameras(); + external_image_capture_ = std::make_unique(&external_cameras_); + if (getApiProvider()->hasDefaultVehicle()) { //TODO: better handle no FPV vehicles scenario getVehicleSimApi()->possess(); @@ -958,11 +891,6 @@ std::string ASimModeBase::getVehiclePawnPathName(const AirSimSettings::VehicleSe //derived class should override this method to retrieve types of pawns they support return ""; } -std::string ASimModeBase::getBeaconPawnPathName(const AirSimSettings::BeaconSetting& beacon_setting) const -{ - //derived class should override this method to retrieve types of pawns they support - return ""; -} PawnEvents* ASimModeBase::getVehiclePawnEvents(APawn* pawn) const { unused(pawn); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index b169d9178..4b799bfbe 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -70,6 +70,7 @@ class AIRSIM_API ASimModeBase : public AActor virtual void continueForFrames(uint32_t frames); virtual void setWind(const msr::airlib::Vector3r& wind) const; + virtual void setExtForce(const msr::airlib::Vector3r& ext_force) const; virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst, float celestial_clock_speed, float update_interval_secs, bool move_sun); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 176cb610f..f86fe850c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -74,6 +74,7 @@ std::unique_ptr ASimModeWorldBase::createP } physics_engine->setWind(getSettings().wind); + physics_engine->setExtForce(getSettings().ext_force); } else if (physics_engine_name == "ExternalPhysicsEngine") { physics_engine.reset(new msr::airlib::ExternalPhysicsEngine()); @@ -136,6 +137,11 @@ void ASimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const physics_engine_->setWind(wind); } +void ASimModeWorldBase::setExtForce(const msr::airlib::Vector3r& ext_force) const +{ + physics_engine_->setExtForce(ext_force); +} + void ASimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter) { unused(debug_reporter); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index bf54f2b5d..37be47fed 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -34,6 +34,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase virtual void continueForFrames(uint32_t frames) override; virtual void setWind(const msr::airlib::Vector3r& wind) const override; + virtual void setExtForce(const msr::airlib::Vector3r& ext_force) const override; protected: void startAsyncUpdator(); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp index db41000a1..50f54a6a1 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.cpp @@ -4,7 +4,7 @@ #include "Components/TextRenderComponent.h" #include "Components/AudioComponent.h" #include "Sound/SoundCue.h" -#include "WheeledVehicleMovementComponent4W.h" +#include "ChaosWheeledVehicleMovementComponent.h" #include "CarWheelFront.h" #include "CarWheelRear.h" @@ -85,32 +85,33 @@ ACarPawn::ACarPawn() void ACarPawn::setupVehicleMovementComponent() { - UWheeledVehicleMovementComponent4W* movement = CastChecked(getVehicleMovementComponent()); + UChaosWheeledVehicleMovementComponent* movement = CastChecked(getVehicleMovementComponent()); + movement->WheelSetups.SetNum(4); check(movement->WheelSetups.Num() == 4); // Wheels/Tires // Setup the wheels movement->WheelSetups[0].WheelClass = UCarWheelFront::StaticClass(); - movement->WheelSetups[0].BoneName = FName("PhysWheel_FL"); + movement->WheelSetups[0].BoneName = FName("WheelFL"); movement->WheelSetups[0].AdditionalOffset = FVector(0.f, -8.f, 0.f); movement->WheelSetups[1].WheelClass = UCarWheelFront::StaticClass(); - movement->WheelSetups[1].BoneName = FName("PhysWheel_FR"); + movement->WheelSetups[1].BoneName = FName("WheelFR"); movement->WheelSetups[1].AdditionalOffset = FVector(0.f, 8.f, 0.f); movement->WheelSetups[2].WheelClass = UCarWheelRear::StaticClass(); - movement->WheelSetups[2].BoneName = FName("PhysWheel_BL"); + movement->WheelSetups[2].BoneName = FName("WheelBL"); movement->WheelSetups[2].AdditionalOffset = FVector(0.f, -8.f, 0.f); movement->WheelSetups[3].WheelClass = UCarWheelRear::StaticClass(); - movement->WheelSetups[3].BoneName = FName("PhysWheel_BR"); + movement->WheelSetups[3].BoneName = FName("WheelBR"); movement->WheelSetups[3].AdditionalOffset = FVector(0.f, 8.f, 0.f); // Adjust the tire loading - movement->MinNormalizedTireLoad = 0.0f; - movement->MinNormalizedTireLoadFiltered = 0.2308f; - movement->MaxNormalizedTireLoad = 2.0f; - movement->MaxNormalizedTireLoadFiltered = 2.0f; + //movement->MinNormalizedTireLoad = 0.0f; + //movement->MinNormalizedTireLoadFiltered = 0.2308f; + //movement->MaxNormalizedTireLoad = 2.0f; + //movement->MaxNormalizedTireLoadFiltered = 2.0f; // Engine // Torque setup @@ -121,22 +122,22 @@ void ACarPawn::setupVehicleMovementComponent() movement->EngineSetup.TorqueCurve.GetRichCurve()->AddKey(5730.0f, 400.0f); // Adjust the steering - movement->SteeringCurve.GetRichCurve()->Reset(); - movement->SteeringCurve.GetRichCurve()->AddKey(0.0f, 1.0f); - movement->SteeringCurve.GetRichCurve()->AddKey(40.0f, 0.7f); - movement->SteeringCurve.GetRichCurve()->AddKey(120.0f, 0.6f); + movement->SteeringSetup.SteeringCurve.GetRichCurve()->Reset(); + movement->SteeringSetup.SteeringCurve.GetRichCurve()->AddKey(0.0f, 1.0f); + movement->SteeringSetup.SteeringCurve.GetRichCurve()->AddKey(40.0f, 0.7f); + movement->SteeringSetup.SteeringCurve.GetRichCurve()->AddKey(120.0f, 0.6f); // Transmission // We want 4wd - movement->DifferentialSetup.DifferentialType = EVehicleDifferential4W::LimitedSlip_4W; + movement->DifferentialSetup.DifferentialType = EVehicleDifferential::AllWheelDrive; // Drive the front wheels a little more than the rear movement->DifferentialSetup.FrontRearSplit = 0.65; // Automatic gearbox - movement->TransmissionSetup.bUseGearAutoBox = true; - movement->TransmissionSetup.GearSwitchTime = 0.15f; - movement->TransmissionSetup.GearAutoBoxLatency = 1.0f; + movement->TransmissionSetup.bUseAutomaticGears = true; + movement->TransmissionSetup.GearChangeTime = 0.15f; + //movement->TransmissionSetup.GearAutoBoxLatency = 1.0f; // Disable reverse as brake, this is needed for SetBreakInput() to take effect movement->bReverseAsBrake = false; @@ -147,10 +148,11 @@ void ACarPawn::setupVehicleMovementComponent() if (primitive) { primitive->BodyInstance.COMNudge = FVector(8.0f, 0.0f, 0.0f); } - + movement->UpdatedPrimitive = primitive; // Set the inertia scale. This controls how the mass of the vehicle is distributed. movement->InertiaTensorScale = FVector(1.0f, 1.333f, 1.2f); - movement->bDeprecatedSpringOffsetMode = true; + //movement->bDeprecatedSpringOffsetMode = true; + movement->bAutoRegisterUpdatedComponent = true; } void ACarPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, class UPrimitiveComponent* OtherComp, bool bSelfMoved, FVector HitLocation, @@ -159,7 +161,7 @@ void ACarPawn::NotifyHit(class UPrimitiveComponent* MyComp, class AActor* Other, pawn_events_.getCollisionSignal().emit(MyComp, Other, OtherComp, bSelfMoved, HitLocation, HitNormal, NormalImpulse, Hit); } -UWheeledVehicleMovementComponent* ACarPawn::getVehicleMovementComponent() const +UChaosVehicleMovementComponent* ACarPawn::getVehicleMovementComponent() const { return GetVehicleMovement(); } @@ -250,8 +252,9 @@ void ACarPawn::Tick(float Delta) updateInCarHUD(); // Pass the engine RPM to the sound component - float RPMToAudioScale = 2500.0f / GetVehicleMovement()->GetEngineMaxRotationSpeed(); - engine_sound_audio_->SetFloatParameter(FName("RPM"), GetVehicleMovement()->GetEngineRotationSpeed() * RPMToAudioScale); + UChaosWheeledVehicleMovementComponent* movement = CastChecked(getVehicleMovementComponent()); + float RPMToAudioScale = 2500.0f / movement->GetEngineMaxRotationSpeed(); + engine_sound_audio_->SetFloatParameter(FName("RPM"), movement->GetEngineRotationSpeed() * RPMToAudioScale); pawn_events_.getPawnTickSignal().emit(Delta); } @@ -283,9 +286,10 @@ void ACarPawn::updateHUDStrings() last_gear_ = (Gear == 0) ? LOCTEXT("N", "N") : FText::AsNumber(Gear); } + UChaosWheeledVehicleMovementComponent* movement = CastChecked(getVehicleMovementComponent()); UAirBlueprintLib::LogMessage(TEXT("Speed: "), last_speed_.ToString(), LogDebugLevel::Informational); UAirBlueprintLib::LogMessage(TEXT("Gear: "), last_gear_.ToString(), LogDebugLevel::Informational); - UAirBlueprintLib::LogMessage(TEXT("RPM: "), FText::AsNumber(GetVehicleMovement()->GetEngineRotationSpeed()).ToString(), LogDebugLevel::Informational); + UAirBlueprintLib::LogMessage(TEXT("RPM: "), FText::AsNumber(movement->GetEngineRotationSpeed()).ToString(), LogDebugLevel::Informational); } void ACarPawn::updateInCarHUD() diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h index 16f12f9c7..f8637b7bc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawn.h @@ -1,7 +1,8 @@ #pragma once #include "CoreMinimal.h" -#include "WheeledVehicle.h" +#include "ChaosWheeledVehicleMovementComponent.h" +#include "WheeledVehiclePawn.h" #include "Components/SkeletalMeshComponent.h" #include "PhysicalMaterials/PhysicalMaterial.h" #include "UObject/ConstructorHelpers.h" @@ -26,7 +27,7 @@ class UInputComponent; class UAudioComponent; UCLASS(config = Game) -class ACarPawn : public AWheeledVehicle +class ACarPawn : public AWheeledVehiclePawn { GENERATED_BODY() @@ -46,7 +47,7 @@ class ACarPawn : public AWheeledVehicle { return &pawn_events_; } - UWheeledVehicleMovementComponent* getVehicleMovementComponent() const; + UChaosVehicleMovementComponent* getVehicleMovementComponent() const; const msr::airlib::CarApiBase::CarControls& getKeyBoardControls() const { return keyboard_controls_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp index 5cfe66318..22997e593 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.cpp @@ -1,13 +1,13 @@ #include "CarPawnApi.h" #include "AirBlueprintLib.h" -#include "PhysXVehicleManager.h" +#include "ChaosVehicleManager.h" CarPawnApi::CarPawnApi(ACarPawn* pawn, const msr::airlib::Kinematics::State* pawn_kinematics, msr::airlib::CarApiBase* vehicle_api) : pawn_(pawn), pawn_kinematics_(pawn_kinematics), vehicle_api_(vehicle_api) { - movement_ = pawn->GetVehicleMovement(); + movement_ = CastChecked(pawn->GetVehicleMovement()); } void CarPawnApi::updateMovement(const msr::airlib::CarApiBase::CarControls& controls) @@ -23,7 +23,7 @@ void CarPawnApi::updateMovement(const msr::airlib::CarApiBase::CarControls& cont movement_->SetSteeringInput(controls.steering); movement_->SetBrakeInput(controls.brake); movement_->SetHandbrakeInput(controls.handbrake); - movement_->SetUseAutoGears(!controls.is_manual_gear); + movement_->SetUseAutomaticGears(!controls.is_manual_gear); } msr::airlib::CarApiBase::CarState CarPawnApi::getCarState() const @@ -56,16 +56,17 @@ void CarPawnApi::reset() movement_->SetActive(true, true); vehicle_api_->setCarControls(msr::airlib::CarApiBase::CarControls()); updateMovement(msr::airlib::CarApiBase::CarControls()); + movement_->ResetVehicleState(); - auto pv = movement_->PVehicle; - if (pv) { - pv->mWheelsDynData.setToRestState(); - } - auto pvd = movement_->PVehicleDrive; - if (pvd) { - pvd->mDriveDynData.setToRestState(); - } - }, true); +// auto pv = movement_->PVehicle; +// if (pv) { +// pv->mWheelsDynData.setToRestState(); +// } +// auto pvd = movement_->PVehicleDrive; +// if (pvd) { +// pvd->mDriveDynData.setToRestState(); +// } + }, true); //UAirBlueprintLib::RunCommandOnGameThread([this, &phys_comps]() { // for (auto* phys_comp : phys_comps) // phys_comp->SetSimulatePhysics(true); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h index 7d156ba79..30dec972e 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnApi.h @@ -1,7 +1,7 @@ #pragma once #include "vehicles/car/api/CarApiBase.hpp" -#include "WheeledVehicleMovementComponent4W.h" +#include "ChaosWheeledVehicleMovementComponent.h" #include "physics/Kinematics.hpp" #include "CarPawn.h" @@ -23,7 +23,7 @@ class CarPawnApi virtual ~CarPawnApi(); private: - UWheeledVehicleMovementComponent* movement_; + UChaosWheeledVehicleMovementComponent* movement_; msr::airlib::CarApiBase::CarControls last_controls_; ACarPawn* pawn_; const msr::airlib::Kinematics::State* pawn_kinematics_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index de64a5348..ba4f2ebfe 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h @@ -1,7 +1,7 @@ #pragma once #include "CoreMinimal.h" -#include "WheeledVehicleMovementComponent4W.h" +#include "ChaosWheeledVehicleMovementComponent.h" #include "CarPawn.h" #include "CarPawnApi.h" diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.cpp index 72afaeea8..634630699 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.cpp @@ -1,26 +1,22 @@ // Copyright 1998-2017 Epic Games, Inc. All Rights Reserved. #include "CarWheelFront.h" -#include "TireConfig.h" #include "UObject/ConstructorHelpers.h" UCarWheelFront::UCarWheelFront() { - ShapeRadius = 18.f; - ShapeWidth = 15.0f; - Mass = 20.0f; - DampingRate = 0.25f; + WheelRadius = 38.f; + WheelWidth = 17.0f; + //WheelMass = 20.0f; bAffectedByHandbrake = false; - SteerAngle = 40.f; + bAffectedBySteering = true; + MaxSteerAngle = 50.f; + AxleType = EAxleType::Front; // Setup suspension forces - SuspensionForceOffset = 0.0f; + SuspensionForceOffset = FVector(0.0f, 0.0f, 0.0f); SuspensionMaxRaise = 10.0f; SuspensionMaxDrop = 10.0f; - SuspensionNaturalFrequency = 9.0f; - SuspensionDampingRatio = 1.05f; - - // Find the tire object and set the data for it - static ConstructorHelpers::FObjectFinder TireData(TEXT("/AirSim/VehicleAdv/Vehicle/WheelData/Vehicle_FrontTireConfig.Vehicle_FrontTireConfig")); - TireConfig = TireData.Object; + //SuspensionNaturalFrequency = 9.0f; + SuspensionDampingRatio = 1.5f; } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.h index fd9d088d9..44f098d11 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelFront.h @@ -3,11 +3,11 @@ #pragma once #include "CoreMinimal.h" -#include "VehicleWheel.h" +#include "ChaosVehicleWheel.h" #include "CarWheelFront.generated.h" UCLASS() -class UCarWheelFront : public UVehicleWheel +class UCarWheelFront : public UChaosVehicleWheel { GENERATED_BODY() diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.cpp index d7ce40aac..30c9caa37 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.cpp @@ -1,24 +1,21 @@ // Copyright 1998-2017 Epic Games, Inc. All Rights Reserved. #include "CarWheelRear.h" -#include "TireConfig.h" +//#include "TireConfig.h" #include "UObject/ConstructorHelpers.h" UCarWheelRear::UCarWheelRear() { - ShapeRadius = 18.f; - ShapeWidth = 15.0f; + WheelRadius = 38.f; + WheelWidth = 17.0f; bAffectedByHandbrake = true; - SteerAngle = 0.f; + MaxSteerAngle = 0.f; + AxleType = EAxleType::Rear; // Setup suspension forces - SuspensionForceOffset = -0.0f; + SuspensionForceOffset = FVector(0.0f, 0.0f, 0.0f); SuspensionMaxRaise = 10.0f; SuspensionMaxDrop = 10.0f; - SuspensionNaturalFrequency = 9.0f; - SuspensionDampingRatio = 1.05f; - - // Find the tire object and set the data for it - static ConstructorHelpers::FObjectFinder TireData(TEXT("/AirSim/VehicleAdv/Vehicle/WheelData/Vehicle_BackTireConfig.Vehicle_BackTireConfig")); - TireConfig = TireData.Object; + //SuspensionNaturalFrequency = 9.0f; + SuspensionDampingRatio = 1.5f; } diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.h index 6bd933747..64b5df89c 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarWheelRear.h @@ -3,11 +3,11 @@ #pragma once #include "CoreMinimal.h" -#include "VehicleWheel.h" +#include "ChaosVehicleWheel.h" #include "CarWheelRear.generated.h" UCLASS() -class UCarWheelRear : public UVehicleWheel +class UCarWheelRear : public UChaosVehicleWheel { GENERATED_BODY() diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 2e30ee602..10edfe48d 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -722,6 +722,11 @@ void WorldSimApi::setWind(const Vector3r& wind) const simmode_->setWind(wind); } +void WorldSimApi::setExtForce(const Vector3r& ext_force) const +{ + simmode_->setExtForce(ext_force); +} + std::vector WorldSimApi::listVehicles() const { std::vector vehicle_names; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 438db844d..9eacb74ea 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -77,6 +77,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool isRecording() const override; virtual void setWind(const Vector3r& wind) const override; + virtual void setExtForce(const Vector3r& ext_force) const override; virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override; virtual std::vector listVehicles() const override; diff --git a/build.cmd b/build.cmd index eb481b904..a119818cb 100644 --- a/build.cmd +++ b/build.cmd @@ -144,9 +144,9 @@ IF NOT EXIST Unreal\Plugins\AirSim\Content\VehicleAdv\SUV\v1.2.0 ( REM %powershell% -command "& { Start-BitsTransfer -Source https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -Destination suv_download_tmp\car_assets.zip }" REM %powershell% -command "& { (New-Object System.Net.WebClient).DownloadFile('https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip', 'suv_download_tmp\car_assets.zip') }" if "%PWSHV7%" == "" ( - %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" + %powershell% -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/CodexLabsLLC/Colosseum/releases/download/v2.0.0-beta.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip }" ) else ( - %powershell% -command "iwr https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip" + %powershell% -command "iwr https://github.com/CodexLabsLLC/Colosseum/releases/download/v2.0.0-beta.0/car_assets.zip -OutFile suv_download_tmp\car_assets.zip" ) @echo off rmdir /S /Q Unreal\Plugins\AirSim\Content\VehicleAdv\SUV diff --git a/ros2/src/airsim_interfaces/CMakeLists.txt b/ros2/src/airsim_interfaces/CMakeLists.txt index 7a5edbb03..9b67ddcdc 100644 --- a/ros2/src/airsim_interfaces/CMakeLists.txt +++ b/ros2/src/airsim_interfaces/CMakeLists.txt @@ -32,7 +32,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/Land.srv" "srv/LandGroup.srv" "srv/Reset.srv" - "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs + "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs LIBRARY_NAME ${PROJECT_NAME} ) ament_export_dependencies(rosidl_default_runtime) diff --git a/ros2/src/airsim_interfaces/package.xml b/ros2/src/airsim_interfaces/package.xml index 9b1943950..b0acec166 100644 --- a/ros2/src/airsim_interfaces/package.xml +++ b/ros2/src/airsim_interfaces/package.xml @@ -11,6 +11,7 @@ ament_cmake rosidl_default_generators + rosidl_default_generators std_msgs geometry_msgs diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index a5126034b..85a3b9820 100755 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -348,6 +348,11 @@ class AirsimROSWrapper rclcpp::TimerBase::SharedPtr airsim_control_update_timer_; rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_; + /// Callback groups + std::vector airsim_img_callback_groups_; + rclcpp::CallbackGroup::SharedPtr airsim_control_callback_group_; + std::vector airsim_lidar_callback_groups_; + typedef std::pair, std::string> airsim_img_request_vehicle_name_pair; std::vector airsim_img_request_vehicle_name_pair_vec_; std::vector image_pub_vec_; diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp index 1003f8556..693bdba03 100644 --- a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp @@ -1,35 +1,19 @@ #include #include "airsim_ros_wrapper.h" -int main(int argc, char** argv) -{ +int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; node_options.automatically_declare_parameters_from_overrides(true); - std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); - std::shared_ptr nh_img = nh->create_sub_node("img"); - std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); + std::shared_ptr nh = rclcpp::Node::make_shared("airsim_node", node_options); + std::shared_ptr nh_img = nh->create_sub_node("img"); + std::shared_ptr nh_lidar = nh->create_sub_node("lidar"); std::string host_ip; nh->get_parameter("host_ip", host_ip); AirsimROSWrapper airsim_ros_wrapper(nh, nh_img, nh_lidar, host_ip); - - if (airsim_ros_wrapper.is_used_img_timer_cb_queue_) { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(nh_img); - while (rclcpp::ok()) { - executor.spin(); - } - } - - if (airsim_ros_wrapper.is_used_lidar_timer_cb_queue_) { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(nh_lidar); - while (rclcpp::ok()) { - executor.spin(); - } - } - - rclcpp::spin(nh); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(nh); + executor.spin(); return 0; } \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index ce3f3390d..e0917b0ea 100755 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -109,7 +109,8 @@ void AirsimROSWrapper::initialize_ros() nh_->declare_parameter("vehicle_name", rclcpp::ParameterValue("")); create_ros_pubs_from_settings_json(); - airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this)); + airsim_control_callback_group_ = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_control_update_timer_ = nh_->create_wall_timer(std::chrono::duration(update_airsim_control_every_n_sec), std::bind(&AirsimROSWrapper::drone_state_timer_cb, this), airsim_control_callback_group_); } void AirsimROSWrapper::create_ros_pubs_from_settings_json() @@ -309,8 +310,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() if (!airsim_img_request_vehicle_name_pair_vec_.empty()) { double update_airsim_img_response_every_n_sec; nh_->get_parameter("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec); - - airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this)); + auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_img_callback_groups_.push_back(cb); + airsim_img_response_timer_ = nh_img_->create_wall_timer(std::chrono::duration(update_airsim_img_response_every_n_sec), std::bind(&AirsimROSWrapper::img_response_timer_cb, this), cb); is_used_img_timer_cb_queue_ = true; } @@ -318,7 +320,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() if (lidar_cnt > 0) { double update_lidar_every_n_sec; nh_->get_parameter("update_lidar_every_n_sec", update_lidar_every_n_sec); - airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this)); + auto cb = nh_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + airsim_lidar_callback_groups_.push_back(cb); + airsim_lidar_update_timer_ = nh_lidar_->create_wall_timer(std::chrono::duration(update_lidar_every_n_sec), std::bind(&AirsimROSWrapper::lidar_timer_cb, this), cb); is_used_lidar_timer_cb_queue_ = true; } diff --git a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp index f3d51fc5d..ddad9a9fb 100644 --- a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp +++ b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp @@ -291,8 +291,8 @@ void PIDPositionController::update_control_cmd_timer_cb() } } - // only compute and send control commands for hovering / moving to pose, if we received a goal at least once in the past - if (got_goal_once_) { + // only compute and send control commands for hovering / moving to pose, if we currently have a goal + if (has_goal_) { compute_control_cmd(); enforce_dynamic_constraints(); publish_control_cmd(); diff --git a/setup.sh b/setup.sh index 0fb1fac6b..896a475ca 100755 --- a/setup.sh +++ b/setup.sh @@ -66,6 +66,10 @@ if [ "$(uname)" == "Darwin" ]; then # osx sudo dseditgroup -o edit -a `whoami` -t user dialout fi + # MacOS 11 has new Python env management that breaks the Python 2-to-3 + # build process. We need to make sure brew updates before attempting to + # install, since it will update packaages + brew update brew_install wget brew_install coreutils @@ -154,7 +158,7 @@ if $downloadHighPolySuv; then fi mkdir -p "suv_download_tmp" cd suv_download_tmp - wget https://github.com/Microsoft/AirSim/releases/download/v1.2.0/car_assets.zip + wget https://github.com/CodexLabsLLC/Colosseum/releases/download/v2.0.0-beta.0/car_assets.zip if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" fi @@ -170,7 +174,7 @@ echo "Installing Eigen library..." if [ ! -d "AirLib/deps/eigen3" ]; then echo "Downloading Eigen..." - wget -O eigen3.zip https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip + wget -O eigen3.zip https://gitlab.com/libeigen/eigen/-/archive/3.4.0/eigen-3.4.0.zip unzip -q eigen3.zip -d temp_eigen mkdir -p AirLib/deps/eigen3 mv temp_eigen/eigen*/Eigen AirLib/deps/eigen3