diff --git a/.gitignore b/.gitignore index 64d80d637e..85669e3f14 100644 --- a/.gitignore +++ b/.gitignore @@ -361,10 +361,6 @@ xcuserdata/ !*.xcworkspace/contents.xcworkspacedata /*.gcno -# Sublime Text -*.sublime-workspace -*.sublime-project - # Unity /Unity/UnityDemo/Assembly-CSharp.csproj /Unity/UnityDemo/UnityDemo.sln @@ -385,17 +381,9 @@ ros/logs/ ros/.catkin_workspace ros/src/CMakeLists.txt -# ROS2 -ros2/install/ -ros2/log/ -ros2/src/log - # docs docs/README.md build_docs/ # api docs PythonClient/docs/_build - -# Docker -/docker/Blocks/ diff --git a/AirLib/include/api/RpcLibAdaptorsBase.hpp b/AirLib/include/api/RpcLibAdaptorsBase.hpp index 4785d443b6..eda2b882ed 100644 --- a/AirLib/include/api/RpcLibAdaptorsBase.hpp +++ b/AirLib/include/api/RpcLibAdaptorsBase.hpp @@ -491,16 +491,22 @@ namespace airlib_rpclib } ImageRequest(const msr::airlib::ImageCaptureBase::ImageRequest& s) - : camera_name(s.camera_name) - , image_type(s.image_type) - , pixels_as_float(s.pixels_as_float) - , compress(s.compress) { + camera_name = s.camera_name; + image_type = s.image_type; + pixels_as_float = s.pixels_as_float; + compress = s.compress; } msr::airlib::ImageCaptureBase::ImageRequest to() const { - return { camera_name, image_type, pixels_as_float, compress }; + msr::airlib::ImageCaptureBase::ImageRequest d; + d.camera_name = camera_name; + d.image_type = image_type; + d.pixels_as_float = pixels_as_float; + d.compress = compress; + + return d; } static std::vector from( diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index cc3914b6a1..56c54183c8 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -65,9 +65,6 @@ namespace airlib Vector3r simGetObjectScale(const std::string& object_name) const; bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true); bool simSetObjectScale(const std::string& object_name, const Vector3r& scale); - std::string simSpawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, - const Vector3r& scale, bool physics_enabled); - bool simDestroyObject(const std::string& object_name); //task management APIs void cancelLastTask(const std::string& vehicle_name = ""); @@ -149,11 +146,8 @@ namespace airlib bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; - void simSetKinematics(const Kinematics::State& state, bool ignore_collision, const std::string& vehicle_name = ""); msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const; std::vector simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0); - bool simSetObjectMaterial(const std::string& object_name, const std::string& material_name); - bool simSetObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path); // Recording APIs void startRecording(); @@ -165,8 +159,6 @@ namespace airlib std::string getSettingsString() const; - std::vector simListAssets() const; - protected: void* getClient(); const void* getClient() const; diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index a2a55f6fe7..ffa329ec6e 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -54,7 +54,6 @@ namespace airlib virtual Pose getPose() const = 0; virtual void setPose(const Pose& pose, bool ignore_collision) = 0; virtual const Kinematics::State* getGroundTruthKinematics() const = 0; - virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) = 0; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0; virtual CollisionInfo getCollisionInfo() const = 0; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index e7fc7c3754..4b165c49d9 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -32,9 +32,8 @@ namespace airlib // ------ Level setting apis ----- // virtual bool loadLevel(const std::string& level_name) = 0; - virtual string spawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled, bool is_blueprint) = 0; - virtual bool destroyObject(const std::string& object_name) = 0; - virtual std::vector listAssets() const = 0; + virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) = 0; + virtual bool destroyObject(const string& object_name) = 0; virtual bool isPaused() const = 0; virtual void reset() = 0; @@ -73,9 +72,6 @@ namespace airlib virtual bool runConsoleCommand(const std::string& command) = 0; virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) = 0; virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) = 0; - virtual bool setLightIntensity(const std::string& light_name, float intensity) = 0; - virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) = 0; - virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) = 0; virtual vector getMeshPositionVertexBuffers() const = 0; 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) = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index a3c8dfd922..a54334bc9f 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -118,7 +118,7 @@ namespace airlib { } - static Rotation nanRotation() noexcept + static Rotation nanRotation() { static const Rotation val(Utils::nan(), Utils::nan(), Utils::nan()); return val; diff --git a/AirLib/include/common/ClockBase.hpp b/AirLib/include/common/ClockBase.hpp index e4a97edcd0..dbfeaec774 100644 --- a/AirLib/include/common/ClockBase.hpp +++ b/AirLib/include/common/ClockBase.hpp @@ -26,8 +26,6 @@ namespace airlib wall_clock_start_ = Utils::getTimeSinceEpochNanos(); } - virtual ~ClockBase() = default; - TTimeDelta elapsedSince(TTimePoint since) const { return elapsedBetween(nowNanos(), since); diff --git a/AirLib/include/common/EarthUtils.hpp b/AirLib/include/common/EarthUtils.hpp index 45d595f846..a8775fa9a0 100644 --- a/AirLib/include/common/EarthUtils.hpp +++ b/AirLib/include/common/EarthUtils.hpp @@ -306,47 +306,6 @@ namespace airlib return GeoPoint(home_geo_point.home_geo_point.latitude, home_geo_point.home_geo_point.longitude, home_geo_point.home_geo_point.altitude - v.z()); } - static Vector3r GeodeticToEcef(const GeoPoint& geo) - { - // Convert geodetic coordinates to ECEF. - // http://code.google.com/p/pysatel/source/browse/trunk/coord.py?r=22 - double lat_rad = Utils::degreesToRadians(geo.latitude); - double lon_rad = Utils::degreesToRadians(geo.longitude); - double xi = sqrt(1 - (6.69437999014 * 0.001) * sin(lat_rad) * sin(lat_rad)); - real_T x = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * cos(lon_rad)); - real_T y = static_cast((EARTH_RADIUS / xi + geo.altitude) * cos(lat_rad) * sin(lon_rad)); - real_T z = static_cast((EARTH_RADIUS / xi * (1 - (6.69437999014 * 0.001)) + geo.altitude) * sin(lat_rad)); - return Vector3r(x, y, z); - } - - static Vector3r EcefToNed(const Vector3r& ecefxyz, const Vector3r& ecefhome, const GeoPoint& geohome) - { - // Converts ECEF coordinate position into local-tangent-plane NED. - // Coordinates relative to given ECEF coordinate frame. - - Vector3r vect, ret; - Matrix3x3r ecef_to_ned_matrix_; - double lat_rad = Utils::degreesToRadians(geohome.latitude); - double lon_rad = Utils::degreesToRadians(geohome.longitude); - vect = ecefxyz - ecefhome; - ecef_to_ned_matrix_(0, 0) = static_cast(-sin(lat_rad) * cos(lon_rad)); - ecef_to_ned_matrix_(0, 1) = static_cast(-sin(lat_rad) * sin(lon_rad)); - ecef_to_ned_matrix_(0, 2) = static_cast(cos(lat_rad)); - ecef_to_ned_matrix_(1, 0) = static_cast(-sin(lon_rad)); - ecef_to_ned_matrix_(1, 1) = static_cast(cos(lon_rad)); - ecef_to_ned_matrix_(1, 2) = 0.0f; - ecef_to_ned_matrix_(2, 0) = static_cast(cos(lat_rad) * cos(lon_rad)); - ecef_to_ned_matrix_(2, 1) = static_cast(cos(lat_rad) * sin(lon_rad)); - ecef_to_ned_matrix_(2, 2) = static_cast(sin(lat_rad)); - ret = ecef_to_ned_matrix_ * vect; - return Vector3r(ret(0), ret(1), -ret(2)); - } - - static Vector3r GeodeticToNed(const GeoPoint& geo, const GeoPoint& geohome) - { - return EcefToNed(GeodeticToEcef(geo), GeodeticToEcef(geohome), geohome); - } - //below are approximate versions and would produce errors of more than 10m for points farther than 1km //for more accurate versions, please use the version in EarthUtils::nedToGeodetic static Vector3r GeodeticToNedFast(const GeoPoint& geo, const GeoPoint& home) diff --git a/AirLib/include/common/FrequencyLimiter.hpp b/AirLib/include/common/FrequencyLimiter.hpp index 099c591688..4c27590740 100644 --- a/AirLib/include/common/FrequencyLimiter.hpp +++ b/AirLib/include/common/FrequencyLimiter.hpp @@ -48,7 +48,6 @@ namespace airlib virtual void failResetUpdateOrdering(std::string err) override { - unused(err); // Do nothing. // Disable checks for reset/update sequence because // this object may get created but not used. diff --git a/AirLib/include/common/ImageCaptureBase.hpp b/AirLib/include/common/ImageCaptureBase.hpp index 3ada547eab..1830969e92 100644 --- a/AirLib/include/common/ImageCaptureBase.hpp +++ b/AirLib/include/common/ImageCaptureBase.hpp @@ -26,8 +26,6 @@ namespace airlib Segmentation, SurfaceNormals, Infrared, - OpticalFlow, - OpticalFlowVis, Count //must be last }; @@ -42,15 +40,12 @@ namespace airlib { } - ImageRequest(const std::string& camera_name_val, - ImageCaptureBase::ImageType image_type_val, - bool pixels_as_float_val = false, - bool compress_val = true) - : camera_name(camera_name_val) - , image_type(image_type_val) - , pixels_as_float(pixels_as_float_val) - , compress(compress_val) + ImageRequest(const std::string& camera_name_val, ImageCaptureBase::ImageType image_type_val, bool pixels_as_float_val = false, bool compress_val = true) { + camera_name = camera_name_val; + image_type = image_type_val; + pixels_as_float = pixels_as_float_val; + compress = compress_val; } }; @@ -72,8 +67,7 @@ namespace airlib public: //methods virtual void getImages(const std::vector& requests, std::vector& responses) const = 0; - virtual ~ImageCaptureBase() = default; }; } } //namespace -#endif +#endif \ No newline at end of file diff --git a/AirLib/include/common/common_utils/Signal.hpp b/AirLib/include/common/common_utils/Signal.hpp index 68c0b3f961..70effafbb9 100644 --- a/AirLib/include/common/common_utils/Signal.hpp +++ b/AirLib/include/common/common_utils/Signal.hpp @@ -44,9 +44,9 @@ class Signal Signal() : current_id_(0) {} - // Currently deleted since unused and there shouldn't be any need for this - Signal(Signal const& other) = delete; - Signal& operator=(Signal const& other) = delete; + // copy creates new signal + Signal(Signal const& other) + : current_id_(0) {} // connects a member function to this Signal template @@ -95,10 +95,16 @@ class Signal } } + // assignment creates new Signal + Signal& operator=(Signal const& other) + { + disconnect_all(); + } + private: mutable std::map> slots_; mutable int current_id_; }; } -#endif /* common_utils_Signal_hpp */ +#endif /* common_utils_Signal_hpp */ \ No newline at end of file diff --git a/AirLib/include/common/common_utils/Utils.hpp b/AirLib/include/common/common_utils/Utils.hpp index b43780354c..456bde9259 100644 --- a/AirLib/include/common/common_utils/Utils.hpp +++ b/AirLib/include/common/common_utils/Utils.hpp @@ -50,7 +50,7 @@ /* This file is collection of routines that can be included in ANY project just - by dropping in common_utils.hpp. Therefore there should not be any dependency + by dropping in common_utils.hpp. Therefore there should not be any dependency in the code below other than STL. The code should be able to compilable on all major platforms. */ @@ -102,8 +102,6 @@ class Utils else std::cerr << message << std::endl; } - - virtual ~Logger() = default; }; static void enableImmediateConsoleFlush() diff --git a/AirLib/include/vehicles/car/CarApiFactory.hpp b/AirLib/include/vehicles/car/CarApiFactory.hpp index 35d6d6c0de..d5a8b42e8f 100644 --- a/AirLib/include/vehicles/car/CarApiFactory.hpp +++ b/AirLib/include/vehicles/car/CarApiFactory.hpp @@ -17,14 +17,15 @@ namespace airlib public: static std::unique_ptr createApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, - const Kinematics::State& state, const Environment& environment) + const Kinematics::State& state, const Environment& environment, + const msr::airlib::GeoPoint& home_geopoint) { if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeArduRover) { - return std::unique_ptr(new ArduRoverApi(vehicle_setting, sensor_factory, state, environment)); + return std::unique_ptr(new ArduRoverApi(vehicle_setting, sensor_factory, state, environment, home_geopoint)); } else if (vehicle_setting->vehicle_type == "" || //default config vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePhysXCar) { - return std::unique_ptr(new PhysXCarApi(vehicle_setting, sensor_factory, state, environment)); + return std::unique_ptr(new PhysXCarApi(vehicle_setting, sensor_factory, state, environment, home_geopoint)); } else throw std::runtime_error(Utils::stringf( diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp index debe4602fc..6729985b2d 100644 --- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -32,8 +32,8 @@ namespace airlib public: ArduRoverApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, - const Kinematics::State& state, const Environment& environment) - : CarApiBase(vehicle_setting, sensor_factory, state, environment), home_geopoint_(environment.getHomeGeoPoint()) + const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) + : CarApiBase(vehicle_setting, sensor_factory, state, environment), home_geopoint_(home_geopoint) { connection_info_ = static_cast(vehicle_setting)->connection_info; sensors_ = &getSensors(); diff --git a/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp index bde4b8062b..c09b99c4ea 100644 --- a/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp @@ -15,8 +15,8 @@ namespace airlib { public: PhysXCarApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, - const Kinematics::State& state, const Environment& environment) - : CarApiBase(vehicle_setting, sensor_factory, state, environment), home_geopoint_(environment.getHomeGeoPoint()) + const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) + : CarApiBase(vehicle_setting, sensor_factory, state, environment), home_geopoint_(home_geopoint) { } diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp index 01a8108d37..ea2fb6b033 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorApiBase.hpp @@ -91,8 +91,6 @@ namespace airlib virtual bool land(float timeout_sec); virtual bool goHome(float timeout_sec); - virtual bool moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead); virtual bool moveByVelocityBodyFrame(float vx, float vy, float vz, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByVelocityZBodyFrame(float vx, float vy, float z, float duration, DrivetrainType drivetrain, const YawMode& yaw_mode); virtual bool moveByMotorPWMs(float front_right_pwm, float rear_left_pwm, float front_left_pwm, float rear_right_pwm, float duration); diff --git a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp index 67dfbca91c..351ed37968 100644 --- a/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp +++ b/AirLib/include/vehicles/multirotor/api/MultirotorRpcLibClient.hpp @@ -26,9 +26,6 @@ namespace airlib MultirotorRpcLibClient* landAsync(float timeout_sec = 60, const std::string& vehicle_name = ""); MultirotorRpcLibClient* goHomeAsync(float timeout_sec = Utils::max(), const std::string& vehicle_name = ""); - MultirotorRpcLibClient* moveToGPSAsync(float latitude, float longitude, float altitude, float velocity, float timeout_sec = Utils::max(), - DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), - float lookahead = -1, float adaptive_lookahead = 1, const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityBodyFrameAsync(float vx, float vy, float vz, float duration, DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom, const YawMode& yaw_mode = YawMode(), const std::string& vehicle_name = ""); MultirotorRpcLibClient* moveByVelocityZBodyFrameAsync(float vx, float vy, float z, float duration, diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp index 0add31cbdd..1c81b256c2 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp @@ -17,8 +17,6 @@ class Axis3 { } - virtual ~Axis3() = default; - //access by index virtual T& operator[](unsigned int index) { @@ -370,4 +368,4 @@ struct PidConfig IntegratorType integrator_type = IntegratorType::Standard; }; -} //namespace +} //namespace \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardClock.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardClock.hpp index d706408260..b873f7d813 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardClock.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardClock.hpp @@ -10,7 +10,5 @@ class IBoardClock public: virtual uint64_t micros() const = 0; virtual uint64_t millis() const = 0; - - virtual ~IBoardClock() = default; }; -} +} \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp index 3ea0e714f0..9756d003e6 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp @@ -11,7 +11,5 @@ class IBoardInputPins virtual float readChannel(uint16_t index) const = 0; //output -1 to 1 virtual bool isRcConnected() const = 0; virtual float getAvgMotorOutput() const = 0; - - virtual ~IBoardInputPins() = default; }; -} +} \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardOutputPins.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardOutputPins.hpp index 908f9fcefd..53e23d37d7 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardOutputPins.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardOutputPins.hpp @@ -10,7 +10,5 @@ class IBoardOutputPins public: virtual void writeOutput(uint16_t index, float val) = 0; //val = -1 to 1 for reversible motors otherwise 0 to 1 virtual void setLed(uint8_t index, int32_t color) = 0; - - virtual ~IBoardOutputPins() = default; }; -} +} \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp index d19e65102f..791cd8591a 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IBoardSensors.hpp @@ -8,7 +8,5 @@ class IBoardSensors public: virtual void readAccel(float accel[3]) const = 0; //accel in m/s^2 virtual void readGyro(float gyro[3]) const = 0; //angular velocity vector rad/sec - - virtual ~IBoardSensors() = default; }; -} +} \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IGoal.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IGoal.hpp index d2227d8614..61bd22b057 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IGoal.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IGoal.hpp @@ -11,8 +11,6 @@ class IGoal public: virtual const Axis4r& getGoalValue() const = 0; virtual const GoalMode& getGoalMode() const = 0; - - virtual ~IGoal() = default; }; -} //namespace +} //namespace \ No newline at end of file diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp index 0f19ad83ae..9b63110aba 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IStateEstimator.hpp @@ -20,7 +20,5 @@ class IStateEstimator virtual GeoPoint getHomeGeoPoint() const = 0; virtual Axis3r transformToBodyFrame(const Axis3r& world_frame_val) const = 0; - - virtual ~IStateEstimator() = default; }; -} +} \ No newline at end of file diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 0a60fac9f0..e81891e2d5 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -246,11 +246,6 @@ __pragma(warning(disable : 4239)) pimpl_->client.call("simSetVehiclePose", RpcLibAdaptorsBase::Pose(pose), ignore_collision, vehicle_name); } - void RpcLibClientBase::simSetKinematics(const Kinematics::State& state, bool ignore_collision, const std::string& vehicle_name) - { - pimpl_->client.call("simSetKinematics", RpcLibAdaptorsBase::KinematicsState(state), ignore_collision, vehicle_name); - } - void RpcLibClientBase::simSetTraceLine(const std::vector& color_rgba, float thickness, const std::string& vehicle_name) { pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name); @@ -502,32 +497,11 @@ __pragma(warning(disable : 4239)) return pimpl_->client.call("simSwapTextures", tags, tex_id, component_id, material_id).as>(); } - bool RpcLibClientBase::simSetObjectMaterial(const std::string& object_name, const std::string& material_name) - { - return pimpl_->client.call("simSetObjectMaterial", object_name, material_name).as(); - } - - bool RpcLibClientBase::simSetObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) - { - return pimpl_->client.call("simSetObjectMaterialFromTexture", object_name, texture_path).as(); - } - bool RpcLibClientBase::simLoadLevel(const string& level_name) { return pimpl_->client.call("simLoadLevel", level_name).as(); } - std::string RpcLibClientBase::simSpawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, - const Vector3r& scale, bool physics_enabled) - { - return pimpl_->client.call("simSpawnObject", object_name, load_component, RpcLibAdaptorsBase::Pose(pose), RpcLibAdaptorsBase::Vector3r(scale), physics_enabled).as(); - } - - bool RpcLibClientBase::simDestroyObject(const std::string& object_name) - { - return pimpl_->client.call("simDestroyObject", object_name).as(); - } - msr::airlib::Vector3r RpcLibClientBase::simGetObjectScale(const std::string& object_name) const { return pimpl_->client.call("simGetObjectScale", object_name).as().to(); @@ -640,11 +614,6 @@ __pragma(warning(disable : 4239)) return pimpl_->client.call("getSettingsString").as(); } - std::vector RpcLibClientBase::simListAssets() const - { - return pimpl_->client.call("simListAssets").as>(); - } - void* RpcLibClientBase::getClient() { return &pimpl_->client; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 8ee4ae6841..3b57cbc36d 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -369,18 +369,14 @@ namespace airlib return getWorldSimApi()->loadLevel(level_name); }); - pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdaptorsBase::Pose& pose, const RpcLibAdaptorsBase::Vector3r& scale, bool physics_enabled, bool is_blueprint) -> string { - return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to(), physics_enabled, is_blueprint); + pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdaptorsBase::Pose& pose, const RpcLibAdaptorsBase::Vector3r& scale, bool physics_enabled) -> string { + return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to(), physics_enabled); }); pimpl_->server.bind("simDestroyObject", [&](const string& object_name) -> bool { return getWorldSimApi()->destroyObject(object_name); }); - pimpl_->server.bind("simListAssets", [&]() -> std::vector { - return getWorldSimApi()->listAssets(); - }); - pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdaptorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdaptorsBase::Pose(pose); @@ -452,23 +448,14 @@ namespace airlib return RpcLibAdaptorsBase::KinematicsState(result); }); - pimpl_->server.bind("simSetKinematics", [&](const RpcLibAdaptorsBase::KinematicsState& state, bool ignore_collision, const std::string& vehicle_name) { - getVehicleSimApi(vehicle_name)->setKinematics(state.to(), ignore_collision); - }); - pimpl_->server.bind("simGetGroundTruthEnvironment", [&](const std::string& vehicle_name) -> RpcLibAdaptorsBase::EnvironmentState { const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState(); return RpcLibAdaptorsBase::EnvironmentState(result); }); - pimpl_->server.bind("simCreateVoxelGrid", [&](const RpcLibAdaptorsBase::Vector3r& position, const int& x, const int& y, const int& z, const float& res, const std::string& output_file) -> bool { return getWorldSimApi()->createVoxelGrid(position.to(), x, y, z, res, output_file); }); - pimpl_->server.bind("simSetLightIntensity", [&](const std::string& light_name, float intensity) -> bool { - return getWorldSimApi()->setLightIntensity(light_name, intensity); - }); - pimpl_->server.bind("cancelLastTask", [&](const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->cancelLastTask(); }); @@ -477,14 +464,6 @@ namespace airlib return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); }); - pimpl_->server.bind("simSetObjectMaterial", [&](const std::string& object_name, const std::string& material_name) -> bool { - return getWorldSimApi()->setObjectMaterial(object_name, material_name); - }); - - pimpl_->server.bind("simSetObjectMaterialFromTexture", [&](const std::string& object_name, const std::string& texture_path) -> bool { - return getWorldSimApi()->setObjectMaterialFromTexture(object_name, texture_path); - }); - pimpl_->server.bind("startRecording", [&]() -> void { getWorldSimApi()->startRecording(); }); diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp index 3329798373..2c5662142d 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp @@ -449,24 +449,6 @@ namespace airlib return waiter.isComplete(); } - bool MultirotorApiBase::moveToGPS(float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, - const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) - { - SingleTaskCall lock(this); - GeoPoint target; - target.latitude = latitude; - target.longitude = longitude; - target.altitude = altitude; - if (!std::isnan(getHomeGeoPoint().latitude) && !std::isnan(getHomeGeoPoint().longitude) && !std::isnan(getHomeGeoPoint().altitude)) { - vector path{ msr::airlib::EarthUtils::GeodeticToNed(target, getHomeGeoPoint()) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); - } - else { - vector path{ Vector3r(getPosition().x(), getPosition().y(), getPosition().z()) }; - return moveOnPath(path, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead); - } - } - bool MultirotorApiBase::moveToPosition(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp index 652d3e6507..73ab097f63 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibClient.cpp @@ -156,13 +156,6 @@ __pragma(warning(disable : 4239)) return this; } - 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); - return this; - } - MultirotorRpcLibClient* MultirotorRpcLibClient::moveToPositionAsync(float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) { diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp index 90a0884776..4fb8cdf8d5 100644 --- a/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp +++ b/AirLib/src/vehicles/multirotor/api/MultirotorRpcLibServer.cpp @@ -89,9 +89,6 @@ namespace airlib MultirotorRpcLibAdaptors::to(path, conv_path); return getVehicleApi(vehicle_name)->moveOnPath(conv_path, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); - (static_cast(getServer()))->bind("moveToGPS", [&](float latitude, float longitude, float altitude, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { - return getVehicleApi(vehicle_name)->moveToGPS(latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); - }); (static_cast(getServer()))->bind("moveToPosition", [&](float x, float y, float z, float velocity, float timeout_sec, DrivetrainType drivetrain, const MultirotorRpcLibAdaptors::YawMode& yaw_mode, float lookahead, float adaptive_lookahead, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->moveToPosition(x, y, z, velocity, timeout_sec, drivetrain, yaw_mode.to(), lookahead, adaptive_lookahead); }); diff --git a/DroneShell/include/SimpleShell.hpp b/DroneShell/include/SimpleShell.hpp index 05322fb9a0..fbc3d16b68 100644 --- a/DroneShell/include/SimpleShell.hpp +++ b/DroneShell/include/SimpleShell.hpp @@ -104,13 +104,11 @@ namespace airlib std::unordered_map switches_; public: - ShellCommand(const std::string& name, const std::string& help) + ShellCommand(std::string name, std::string help) : name_(name), help_(help) { } - virtual ~ShellCommand() = default; - void addSwitch(const ShellCommandSwitch& s) { std::string lower = Utils::toLower(s.name); @@ -445,8 +443,6 @@ namespace airlib linenoise::SetCompletionCallback(std::bind(&SimpleShell::commandCompletitionCallBack, this, std::placeholders::_1, std::placeholders::_2)); } - virtual ~SimpleShell() = default; - // add a reference to a command (this object must remain valid, we do not copy it) void addCommand(ShellCommand& command) { diff --git a/MavLinkCom/MavLinkTest/Commands.h b/MavLinkCom/MavLinkTest/Commands.h index 1d209c0235..93b4431fb3 100644 --- a/MavLinkCom/MavLinkTest/Commands.h +++ b/MavLinkCom/MavLinkTest/Commands.h @@ -53,7 +53,7 @@ class Command { public: Command(); - virtual ~Command(); + ~Command(); std::string Name; diff --git a/MavLinkCom/include/MavLinkDebugLog.hpp b/MavLinkCom/include/MavLinkDebugLog.hpp index e19bcc084f..14e2cceb7b 100644 --- a/MavLinkCom/include/MavLinkDebugLog.hpp +++ b/MavLinkCom/include/MavLinkDebugLog.hpp @@ -32,9 +32,7 @@ class MavLinkDebugLog return logger_; } - - virtual ~MavLinkDebugLog() = default; }; } -#endif +#endif \ No newline at end of file diff --git a/MavLinkCom/include/MavLinkMessageBase.hpp b/MavLinkCom/include/MavLinkMessageBase.hpp index c9471cac0e..f17c931e1b 100644 --- a/MavLinkCom/include/MavLinkMessageBase.hpp +++ b/MavLinkCom/include/MavLinkMessageBase.hpp @@ -118,7 +118,6 @@ class MavLinkCommand { public: uint16_t command = 0; - virtual ~MavLinkCommand() = default; protected: virtual void pack() = 0; diff --git a/MavLinkCom/src/MavLinkMessageBase.cpp b/MavLinkCom/src/MavLinkMessageBase.cpp index effcbce739..26c8c0a7ed 100644 --- a/MavLinkCom/src/MavLinkMessageBase.cpp +++ b/MavLinkCom/src/MavLinkMessageBase.cpp @@ -411,4 +411,4 @@ std::string MavLinkMessageBase::float_array_tostring(int len, const float* field } } return line.str(); -} +} \ No newline at end of file diff --git a/MavLinkCom/src/impl/AdHocConnectionImpl.cpp b/MavLinkCom/src/impl/AdHocConnectionImpl.cpp index 6c68da5f0c..b8b34d5b3c 100644 --- a/MavLinkCom/src/impl/AdHocConnectionImpl.cpp +++ b/MavLinkCom/src/impl/AdHocConnectionImpl.cpp @@ -153,7 +153,7 @@ void AdHocConnectionImpl::sendMessage(const std::vector& msg) int AdHocConnectionImpl::subscribe(AdHocMessageHandler handler) { - MessageHandlerEntry entry{ static_cast(listeners.size() + 1), handler }; + MessageHandlerEntry entry = { static_cast(listeners.size() + 1), handler = handler }; std::lock_guard guard(listener_mutex); listeners.push_back(entry); snapshot_stale = true; diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index 61dd8cc604..b44ef354d6 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -355,7 +355,7 @@ void MavLinkConnectionImpl::sendMessage(const MavLinkMessageBase& msg) int MavLinkConnectionImpl::subscribe(MessageHandler handler) { - MessageHandlerEntry entry{ static_cast(listeners.size() + 1), handler }; + MessageHandlerEntry entry = { static_cast(listeners.size() + 1), handler = handler }; std::lock_guard guard(listener_mutex); listeners.push_back(entry); snapshot_stale = true; diff --git a/MavLinkCom/src/serial_com/Port.h b/MavLinkCom/src/serial_com/Port.h index 90c8270dfe..8c8bdcb07a 100644 --- a/MavLinkCom/src/serial_com/Port.h +++ b/MavLinkCom/src/serial_com/Port.h @@ -21,7 +21,5 @@ class Port virtual bool isClosed() = 0; virtual int getRssi(const char* ifaceName) = 0; - - virtual ~Port() = default; }; #endif // !PORT_H diff --git a/MavLinkCom/src/serial_com/SerialPort.hpp b/MavLinkCom/src/serial_com/SerialPort.hpp index e399d1a402..94d8d0350f 100644 --- a/MavLinkCom/src/serial_com/SerialPort.hpp +++ b/MavLinkCom/src/serial_com/SerialPort.hpp @@ -38,7 +38,7 @@ class SerialPort : public Port { public: SerialPort(); - ~SerialPort(); + virtual ~SerialPort(); // open the serial port virtual int connect(const char* portName, int baudRate); @@ -67,4 +67,4 @@ class SerialPort : public Port std::unique_ptr impl_; }; -#endif +#endif \ No newline at end of file diff --git a/MavLinkCom/src/serial_com/TcpClientPort.hpp b/MavLinkCom/src/serial_com/TcpClientPort.hpp index b462448621..490f970f5a 100644 --- a/MavLinkCom/src/serial_com/TcpClientPort.hpp +++ b/MavLinkCom/src/serial_com/TcpClientPort.hpp @@ -10,7 +10,7 @@ class TcpClientPort : public Port { public: TcpClientPort(); - ~TcpClientPort(); + virtual ~TcpClientPort(); // Connect can set you up two different ways. Pass 0 for local port to get any free local // port. localHost allows you to be specific about which local adapter to use in case you diff --git a/MavLinkCom/src/serial_com/UdpClientPort.hpp b/MavLinkCom/src/serial_com/UdpClientPort.hpp index faf33b0ae9..9b094b9540 100644 --- a/MavLinkCom/src/serial_com/UdpClientPort.hpp +++ b/MavLinkCom/src/serial_com/UdpClientPort.hpp @@ -10,7 +10,7 @@ class UdpClientPort : public Port { public: UdpClientPort(); - ~UdpClientPort(); + virtual ~UdpClientPort(); // Connect can set you up two different ways. Pass 0 for local port to get any free local // port and pass a fixed remotePort if you want to send to a specific remote port. diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index fbbdebe3fc..51f4df9d4e 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -110,7 +110,7 @@ def simContinueForTime(self, seconds): seconds (float): Time to run the simulation for """ 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. @@ -158,19 +158,6 @@ def confirmConnection(self): print(ver_info) print('') - def simSetLightIntensity(self, light_name, intensity): - """ - Change intensity of named light - - Args: - light_name (str): Name of light to change - intensity (float): New intensity value - - Returns: - bool: True if successful, otherwise False - """ - return self.client.call("simSetLightIntensity", light_name, intensity) - def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0): """ Runtime Swap Texture API @@ -191,35 +178,6 @@ def simSwapTextures(self, tags, tex_id = 0, component_id = 0, material_id = 0): return self.client.call("simSwapTextures", tags, tex_id, component_id, material_id) #time - of - day control - def simSetObjectMaterial(self, object_name, material_name): - """ - Runtime Swap Texture API - See https://microsoft.github.io/AirSim/retexturing/ for details - Args: - object_name (str): name of object to set material for - material_name (str): name of material to set for object - - Returns: - bool: True if material was set - """ - return self.client.call("simSetObjectMaterial", object_name, material_name) - - def simSetObjectMaterialFromTexture(self, object_name, texture_path): - """ - Runtime Swap Texture API - See https://microsoft.github.io/AirSim/retexturing/ for details - Args: - object_name (str): name of object to set material for - texture_path (str): path to texture to set for object - - Returns: - bool: True if material was set - """ - return self.client.call("simSetObjectMaterialFromTexture", object_name, texture_path) - - - # time-of-day control - def simSetTimeOfDay(self, is_enabled, start_datetime = "", is_start_datetime_dst = False, celestial_clock_speed = 1, update_interval_secs = 60, move_sun = True): """ Control the position of Sun in the environment @@ -375,7 +333,6 @@ def simGetCurrentFieldOfView(self, camera_name, vehicle_name = '', external = Fa return result #End CinemAirSim - def simTestLineOfSightToPoint(self, point, vehicle_name = ''): """ Returns whether the target point is visible from the perspective of the inputted vehicle @@ -388,7 +345,7 @@ def simTestLineOfSightToPoint(self, point, vehicle_name = ''): [bool]: Success """ return self.client.call('simTestLineOfSightToPoint', point, vehicle_name) - + def simTestLineOfSightBetweenPoints(self, point1, point2): """ Returns whether the target point is visible from the perspective of the source point @@ -401,7 +358,7 @@ def simTestLineOfSightBetweenPoints(self, point1, point2): [bool]: Success """ return self.client.call('simTestLineOfSightBetweenPoints', point1, point2) - + def simGetWorldExtents(self): """ Returns a list of GeoPoints representing the minimum and maximum extents of the world @@ -553,7 +510,7 @@ def simListSceneObjects(self, name_regex = '.*'): list[str]: List containing all the names """ return self.client.call('simListSceneObjects', name_regex) - + def simLoadLevel(self, level_name): """ Loads a level specified by its name @@ -566,37 +523,26 @@ def simLoadLevel(self, level_name): """ 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): + def simSpawnObject(self, object_name, asset_name, pose, scale, physics_enabled=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) + return self.client.call('simSpawnObject', object_name, asset_name, pose, scale, physics_enabled) 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 """ @@ -646,7 +592,7 @@ def simAddDetectionFilterMeshName(self, camera_name, image_type, mesh_name, vehi """ self.client.call('simAddDetectionFilterMeshName', camera_name, image_type, mesh_name, vehicle_name, external) - + def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicle_name = '', external = False): """ Set detection radius for all cameras @@ -659,7 +605,7 @@ def simSetDetectionFilterRadius(self, camera_name, image_type, radius_cm, vehicl external (bool, optional): Whether the camera is an External Camera """ self.client.call('simSetDetectionFilterRadius', camera_name, image_type, radius_cm, vehicle_name, external) - + def simClearDetectionMeshNames(self, camera_name, image_type, vehicle_name = '', external = False): """ Clear all mesh names from detection filter @@ -733,7 +679,7 @@ def simGetDistortionParams(self, camera_name, vehicle_name = '', external = Fals Returns: List (float): List of distortion parameter values corresponding to K1, K2, K3, P1, P2 respectively. """ - + return self.client.call('simGetDistortionParams', str(camera_name), vehicle_name, external) def simSetDistortionParams(self, camera_name, distortion_params, vehicle_name = '', external = False): @@ -804,19 +750,6 @@ def simGetGroundTruthKinematics(self, vehicle_name = ''): return KinematicsState.from_msgpack(kinematics_state) simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState} - def simSetKinematics(self, state, ignore_collision, vehicle_name = ''): - """ - Set the kinematics state of the vehicle - - If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values - - Args: - state (KinematicsState): Desired Pose pf the vehicle - ignore_collision (bool): Whether to ignore any collision or not - vehicle_name (str, optional): Name of the vehicle to move - """ - self.client.call('simSetKinematics', state, ignore_collision, vehicle_name) - def simGetGroundTruthEnvironment(self, vehicle_name = ''): """ Get ground truth environment state @@ -1055,7 +988,7 @@ 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 + wind (Vector3r): Wind, in World frame, NED direction, in m/s """ self.client.call('simSetWind', wind) @@ -1219,10 +1152,6 @@ def moveToPositionAsync(self, x, y, z, velocity, timeout_sec = 3e+38, drivetrain lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToPosition', x, y, z, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) - def moveToGPSAsync(self, latitude, longitude, altitude, velocity, timeout_sec = 3e+38, drivetrain = DrivetrainType.MaxDegreeOfFreedom, yaw_mode = YawMode(), - lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): - return self.client.call_async('moveToGPS', latitude, longitude, altitude, velocity, timeout_sec, drivetrain, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) - def moveToZAsync(self, z, velocity, timeout_sec = 3e+38, yaw_mode = YawMode(), lookahead = -1, adaptive_lookahead = 1, vehicle_name = ''): return self.client.call_async('moveToZ', z, velocity, timeout_sec, yaw_mode, lookahead, adaptive_lookahead, vehicle_name) @@ -1314,7 +1243,7 @@ def moveByRollPitchYawZAsync(self, roll, pitch, yaw, z, duration, vehicle_name = def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, vehicle_name = ''): """ - Desired throttle is between 0.0 to 1.0 - - Roll angle, pitch angle, and yaw angle are given in **degrees** when using PX4 and in **radians** when using SimpleFlight, in the body frame. + - Roll angle, pitch angle, and yaw angle are given in **radians**, in the body frame. - The body frame follows the Front Left Up (FLU) convention, and right-handedness. - Frame Convention: @@ -1334,9 +1263,9 @@ def moveByRollPitchYawThrottleAsync(self, roll, pitch, yaw, throttle, duration, | Hence, yawing with a positive angle is equivalent to rotated towards the **left** direction wrt our FLU body frame. Or in an anticlockwise fashion in the body XY / FL plane. Args: - roll (float): Desired roll angle. - pitch (float): Desired pitch angle. - yaw (float): Desired yaw angle. + roll (float): Desired roll angle, in radians. + pitch (float): Desired pitch angle, in radians. + yaw (float): Desired yaw angle, in radians. throttle (float): Desired throttle (between 0.0 to 1.0) duration (float): Desired amount of time (seconds), to send this command for vehicle_name (str, optional): Name of the multirotor to send this command to diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 7aef005549..08c099224d 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -37,10 +37,6 @@ def SurfaceNormals(cls): return 6 def Infrared(cls): return 7 - def OpticalFlow(cls): - return 8 - def OpticalFlowVis(cls): - return 9 def __getattr__(self, key): if key == 'DepthPlanner': @@ -56,8 +52,6 @@ class ImageType(metaclass=_ImageType): Segmentation = 5 SurfaceNormals = 6 Infrared = 7 - OpticalFlow = 8 - OpticalFlowVis = 9 class DrivetrainType: MaxDegreeOfFreedom = 0 diff --git a/PythonClient/car/pause_test.py b/PythonClient/car/pause_test.py deleted file mode 100644 index e8791871a6..0000000000 --- a/PythonClient/car/pause_test.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -import airsim -import time -import numpy as np - -# connect to the AirSim simulator -client = airsim.CarClient() -client.confirmConnection() -client.enableApiControl(True) -car_controls = airsim.CarControls() - -# set the controls for car -car_controls.throttle = -0.5 -car_controls.is_manual_gear = True -car_controls.manual_gear = -1 -client.setCarControls(car_controls) - -# let car drive a bit -time.sleep(10) - -client.simPause(True) -car_position1 = client.getCarState().kinematics_estimated.position -img_position1 = client.simGetImages([airsim.ImageRequest(0, airsim.ImageType.Scene)])[0].camera_position -print(f"Before pause position: {car_position1}") -print(f"Before pause diff: {car_position1.x_val - img_position1.x_val}, {car_position1.y_val - img_position1.y_val}, {car_position1.z_val - img_position1.z_val}") - -time.sleep(10) - -car_position2 = client.getCarState().kinematics_estimated.position -img_position2 = client.simGetImages([airsim.ImageRequest(0, airsim.ImageType.Scene)])[0].camera_position -print(f"After pause position: {car_position2}") -print(f"After pause diff: {car_position2.x_val - img_position2.x_val}, {car_position2.y_val - img_position2.y_val}, {car_position2.z_val - img_position2.z_val}") -client.simPause(False) diff --git a/PythonClient/car/runtime_car.py b/PythonClient/car/runtime_car.py deleted file mode 100644 index 015cf90a42..0000000000 --- a/PythonClient/car/runtime_car.py +++ /dev/null @@ -1,75 +0,0 @@ -import setup_path -import airsim -import time -import sys -import threading - - -def runSingleCar(id: int): - client = airsim.CarClient() - client.confirmConnection() - - vehicle_name = f"Car_{id}" - pose = airsim.Pose(airsim.Vector3r(0, 7*id, 0), - airsim.Quaternionr(0, 0, 0, 0)) - - print(f"Creating {vehicle_name}") - success = client.simAddVehicle(vehicle_name, "Physxcar", pose) - - if not success: - print(f"Falied to create {vehicle_name}") - return - - # Sleep for some time to wait for other vehicles to be created - time.sleep(1) - - # driveCar(vehicle_name, client) - print(f"Driving {vehicle_name} for a few secs...") - client.enableApiControl(True, vehicle_name) - - car_controls = airsim.CarControls() - - # go forward - car_controls.throttle = 0.5 - car_controls.steering = 0 - client.setCarControls(car_controls, vehicle_name) - time.sleep(3) # let car drive a bit - - # Go forward + steer right - car_controls.throttle = 0.5 - car_controls.steering = 1 - client.setCarControls(car_controls, vehicle_name) - time.sleep(3) - - # go reverse - car_controls.throttle = -0.5 - car_controls.is_manual_gear = True - car_controls.manual_gear = -1 - car_controls.steering = 0 - client.setCarControls(car_controls, vehicle_name) - time.sleep(3) - car_controls.is_manual_gear = False # change back gear to auto - car_controls.manual_gear = 0 - - # apply brakes - car_controls.brake = 1 - client.setCarControls(car_controls, vehicle_name) - time.sleep(3) - - -if __name__ == "__main__": - num_vehicles = 3 - - if len(sys.argv) == 2: - num_vehicles = int(sys.argv[1]) - - print(f"Creating {num_vehicles} vehicles") - - threads = [] - for id in range(num_vehicles, 0, -1): - t = threading.Thread(target=runSingleCar, args=(id,)) - threads.append(t) - t.start() - - for t in threads: - t.join() diff --git a/PythonClient/environment/change_texture_example.py b/PythonClient/environment/change_texture_example.py deleted file mode 100644 index b10a8aff51..0000000000 --- a/PythonClient/environment/change_texture_example.py +++ /dev/null @@ -1,7 +0,0 @@ -import airsim - -c = airsim.MultirotorClient() -c.confirmConnection() - -c.simSetObjectMaterialFromTexture("OrangeBall", "sample_texture.jpg") - diff --git a/PythonClient/environment/create_objects.py b/PythonClient/environment/create_objects.py deleted file mode 100644 index 5dc445d1ce..0000000000 --- a/PythonClient/environment/create_objects.py +++ /dev/null @@ -1,32 +0,0 @@ -import setup_path -import airsim -import random -import time - -client = airsim.VehicleClient() -client.confirmConnection() - -assets = client.simListAssets() -print(f"Assets: {assets}") - -scale = airsim.Vector3r(1.0, 1.0, 1.0) - -# asset_name = random.choice(assets) -asset_name = '1M_Cube_Chamfer' - -desired_name = f"{asset_name}_spawn_{random.randint(0, 100)}" -pose = airsim.Pose(position_val=airsim.Vector3r(5.0, 0.0, 0.0)) - -obj_name = client.simSpawnObject(desired_name, asset_name, pose, scale, True) - -print(f"Created object {obj_name} from asset {asset_name} " - f"at pose {pose}, scale {scale}") - -all_objects = client.simListSceneObjects() -if obj_name not in all_objects: - print(f"Object {obj_name} not present!") - -time.sleep(10.0) - -print(f"Destroying {obj_name}") -client.simDestroyObject(obj_name) diff --git a/PythonClient/environment/light_control.py b/PythonClient/environment/light_control.py deleted file mode 100644 index 39fa0f06ed..0000000000 --- a/PythonClient/environment/light_control.py +++ /dev/null @@ -1,23 +0,0 @@ -import airsim -import time - -client = airsim.VehicleClient() -client.confirmConnection() - -# Access an existing light in the world -lights = client.simListSceneObjects("PointLight.*") -pose = client.simGetObjectPose(lights[0]) -scale = airsim.Vector3r(1, 1, 1) - -# Destroy the light -client.simDestroyObject(lights[0]) -time.sleep(1) - -# Create a new light at the same pose -new_light_name = client.simSpawnObject("PointLight", "PointLightBP", pose, scale, False, True) -time.sleep(1) - -# Change the light's intensity -for i in range(20): - client.simSetLightIntensity(new_light_name, i * 100) - time.sleep(0.5) diff --git a/PythonClient/environment/sample_texture.jpg b/PythonClient/environment/sample_texture.jpg deleted file mode 100644 index 9081250511..0000000000 Binary files a/PythonClient/environment/sample_texture.jpg and /dev/null differ diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index bd1c05e527..b51084e70d 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -226,12 +226,6 @@ const msr::airlib::Kinematics::State* PawnSimApi::getGroundTruthKinematics() con return &kinematics_->getState(); } -void PawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision) -{ - unused(ignore_collision); - return kinematics_->setState(state); -} - const msr::airlib::Environment* PawnSimApi::getGroundTruthEnvironment() const { return environment_.get(); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index 7c82da980f..83cb525bf8 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -85,7 +85,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override; - virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override; virtual std::string getRecordFileLine(bool is_header_line) const override; virtual void reportState(msr::airlib::StateReporter& reporter) override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp index 7cd077a999..c2188e408f 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -7,7 +7,7 @@ #include "../../UnitySensors/UnitySensorFactory.h" CarPawnSimApi::CarPawnSimApi(const Params& params, std::string car_name) - : PawnSimApi(params), car_name_(car_name) + : PawnSimApi(params), params_(params), car_name_(car_name) { } @@ -15,18 +15,20 @@ void CarPawnSimApi::initialize() { PawnSimApi::initialize(); - std::shared_ptr sensor_factory = std::make_shared(car_name_, &getNedTransform()); - - vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), - sensor_factory, - *getGroundTruthKinematics(), - *getGroundTruthEnvironment()); - pawn_api_ = std::unique_ptr(new CarPawnApi(getGroundTruthKinematics(), car_name_, vehicle_api_.get())); + createVehicleApi(params_.home_geopoint); //TODO: should do reset() here? joystick_controls_ = msr::airlib::CarApiBase::CarControls(); } +void CarPawnSimApi::createVehicleApi(const msr::airlib::GeoPoint& home_geopoint) +{ + std::shared_ptr sensor_factory = std::make_shared(car_name_, &getNedTransform()); + + vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), sensor_factory, (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()), home_geopoint); + pawn_api_ = std::unique_ptr(new CarPawnApi(getGroundTruthKinematics(), car_name_, vehicle_api_.get())); +} + std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const { std::string common_line = PawnSimApi::getRecordFileLine(is_header_line); diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h index ee7360a79b..da9b2b948a 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Car/CarPawnSimApi.h @@ -14,6 +14,7 @@ class CarPawnSimApi : public PawnSimApi typedef msr::airlib::Pose Pose; private: + void createVehicleApi(const msr::airlib::GeoPoint& home_geopoint); void updateCarControls(); public: @@ -36,6 +37,8 @@ class CarPawnSimApi : public PawnSimApi virtual void resetImplementation() override; private: + Params params_; + std::unique_ptr vehicle_api_; std::unique_ptr pawn_api_; std::vector vehicle_api_messages_; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index ecc5cafb0d..517fccc2eb 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -125,14 +125,6 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision) pending_pose_status_ = PendingPoseStatus::RenderStatePending; } -void MultirotorPawnSimApi::setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) -{ - PawnSimApi::setKinematics(state, ignore_collision); - - msr::airlib::Pose pose(state.pose.position, state.pose.orientation); - setPose(pose, ignore_collision); -} - //*** Start: UpdatableState implementation ***// void MultirotorPawnSimApi::resetImplementation() { diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index 72bacda6db..3e63e39e39 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -30,7 +30,6 @@ class MultirotorPawnSimApi : public PawnSimApi virtual void reportState(StateReporter& reporter) override; virtual UpdatableObject* getPhysicsBody() override; virtual void setPose(const Pose& pose, bool ignore_collision) override; - virtual void setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) override; msr::airlib::MultirotorApiBase* getVehicleApi() const { diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 6bdbe80c5a..ff23a8688a 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -58,14 +58,6 @@ void WorldSimApi::printLogMessage(const std::string& message, const std::string& PrintLogMessage(message.c_str(), message_param.c_str(), "", severity); } -bool WorldSimApi::setLightIntensity(const std::string& light_name, float intensity) -{ - throw std::invalid_argument(common_utils::Utils::stringf( - "setLightIntensity is not supported on unity") - .c_str()); - return false; -} - std::unique_ptr> WorldSimApi::swapTextures(const std::string& tag, int tex_id, int component_id, int material_id) { std::unique_ptr> result; @@ -75,22 +67,6 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s return result; } -bool WorldSimApi::setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) -{ - throw std::invalid_argument(common_utils::Utils::stringf( - "setObjectMaterialFromTexture is not supported on unity") - .c_str()); - return false; -} - -bool WorldSimApi::setObjectMaterial(const std::string& object_name, const std::string& material_name) -{ - throw std::invalid_argument(common_utils::Utils::stringf( - "setObjectMaterial is not supported on unity") - .c_str()); - return false; -} - std::vector WorldSimApi::listSceneObjects(const std::string& name_regex) const { std::vector result; @@ -529,12 +505,4 @@ std::vector WorldSimApi::getDetections(ImageCaptureB return std::vector(); } -std::vector WorldSimApi::listAssets() const -{ - throw std::invalid_argument(common_utils::Utils::stringf( - "listAssets API is not supported on Unity") - .c_str()); - return {}; -} - #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index def2b04ed8..6fa183f6fe 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -18,9 +18,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase // ------ Level setting apis ----- // virtual bool loadLevel(const std::string& level_name) override { return false; }; - virtual std::string spawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled, bool is_blueprint) override { return ""; }; + virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled) override { return ""; }; virtual bool destroyObject(const std::string& object_name) override { return false; }; - virtual std::vector listAssets() const override; virtual bool isPaused() const override; virtual void reset() override; @@ -38,10 +37,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual bool setLightIntensity(const std::string& light_name, float intensity) override; virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; - virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) override; - virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; diff --git a/Unity/build.cmd b/Unity/build.cmd index 662e42076f..e2a5b2d511 100644 --- a/Unity/build.cmd +++ b/Unity/build.cmd @@ -1,4 +1,3 @@ -@echo off REM //---------- copy binaries and include for MavLinkCom in deps ---------- msbuild AirLibWrapper\AirsimWrapper.sln /target:Clean /target:Build /property:Configuration=Release /property:Platform=x64 if ERRORLEVEL 1 goto :buildfailed diff --git a/Unreal/Environments/Blocks/.gitignore b/Unreal/Environments/Blocks/.gitignore index 241bf414c4..0c04c95966 100644 --- a/Unreal/Environments/Blocks/.gitignore +++ b/Unreal/Environments/Blocks/.gitignore @@ -1,7 +1,5 @@ # don't check-in Plugins folder we copied from AirSim folder Plugins/ -# Don't add packaged binaries -Packaged/ # don't check-in generated files *.sln @@ -17,4 +15,4 @@ Packaged/ /Makefile # avoid checking uproject because this is usually just version change -*.uproject +*.uproject \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/BP_PIPCamera.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/BP_PIPCamera.uasset index cc1d325496..211f883e3f 100644 Binary files a/Unreal/Plugins/AirSim/Content/Blueprints/BP_PIPCamera.uasset and b/Unreal/Plugins/AirSim/Content/Blueprints/BP_PIPCamera.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/Lights/PointLightBP.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/Lights/PointLightBP.uasset deleted file mode 100644 index 88a3089519..0000000000 Binary files a/Unreal/Plugins/AirSim/Content/Blueprints/Lights/PointLightBP.uasset and /dev/null differ diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/Lights/SpotLightBP.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/Lights/SpotLightBP.uasset deleted file mode 100644 index d8addfcbf6..0000000000 Binary files a/Unreal/Plugins/AirSim/Content/Blueprints/Lights/SpotLightBP.uasset and /dev/null differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/DomainRandomizationMaterial.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/DomainRandomizationMaterial.uasset deleted file mode 100644 index a52dc9cf33..0000000000 Binary files a/Unreal/Plugins/AirSim/Content/HUDAssets/DomainRandomizationMaterial.uasset and /dev/null differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowMaterial.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowMaterial.uasset deleted file mode 100644 index 4a8f9e9106..0000000000 Binary files a/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowMaterial.uasset and /dev/null differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowRGBMaterial.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowRGBMaterial.uasset deleted file mode 100644 index 11cf3d710d..0000000000 Binary files a/Unreal/Plugins/AirSim/Content/HUDAssets/OpticalFlowRGBMaterial.uasset and /dev/null differ diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index e7289d6998..8d36a6659c 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -237,7 +237,6 @@ void UAirBlueprintLib::GenerateAssetRegistryMap(const UObject* context, TMapGetFName()); - Filter.ClassNames.Add(UBlueprint::StaticClass()->GetFName()); Filter.bRecursivePaths = true; auto world = context->GetWorld(); @@ -248,7 +247,7 @@ void UAirBlueprintLib::GenerateAssetRegistryMap(const UObject* context, TMaponViewModeChanged(nodisplay); + if (backup_camera_) + backup_camera_->onViewModeChanged(nodisplay); + if (ExternalCamera) + ExternalCamera->onViewModeChanged(nodisplay); + if (front_camera_) + front_camera_->onViewModeChanged(nodisplay); + UWorld* world = GetWorld(); UGameViewportClient* gameViewport = world->GetGameViewport(); - gameViewport->bDisableWorldRendering = nodisplay; + if (mode_ == ECameraDirectorMode::CAMERA_DIRECTOR_MODE_NODISPLAY) { + gameViewport->bDisableWorldRendering = 1; + } + else { + gameViewport->bDisableWorldRendering = 0; + } } diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 09943c8c5c..f7c5d9c1db 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -44,8 +44,6 @@ APIPCamera::APIPCamera(const FObjectInitializer& ObjectInitializer) image_type_to_pixel_format_map_.Add(5, EPixelFormat::PF_B8G8R8A8); image_type_to_pixel_format_map_.Add(6, EPixelFormat::PF_B8G8R8A8); image_type_to_pixel_format_map_.Add(7, EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(8, EPixelFormat::PF_B8G8R8A8); - image_type_to_pixel_format_map_.Add(9, EPixelFormat::PF_B8G8R8A8); object_filter_ = FObjectFilter(); } @@ -76,10 +74,6 @@ void APIPCamera::PostInitializeComponents() UAirBlueprintLib::GetActorComponent(this, TEXT("InfraredCaptureComponent")); captures_[Utils::toNumeric(ImageType::SurfaceNormals)] = UAirBlueprintLib::GetActorComponent(this, TEXT("NormalsCaptureComponent")); - captures_[Utils::toNumeric(ImageType::OpticalFlow)] = - UAirBlueprintLib::GetActorComponent(this, TEXT("OpticalFlowCaptureComponent")); - captures_[Utils::toNumeric(ImageType::OpticalFlowVis)] = - UAirBlueprintLib::GetActorComponent(this, TEXT("OpticalFlowVisCaptureComponent")); for (unsigned int i = 0; i < imageTypeCount(); ++i) { detections_[i] = NewObject(this); @@ -108,9 +102,7 @@ void APIPCamera::BeginPlay() render_targets_[image_type] = NewObject(); } - //We set all cameras to start as nodisplay - //This improves performance because the capture components are no longer updating every frame and only update while requesting an image - onViewModeChanged(true); + onViewModeChanged(false); gimbal_stabilization_ = 0; gimbald_rotator_ = this->GetActorRotation(); @@ -282,20 +274,6 @@ void APIPCamera::setCameraTypeEnabled(ImageType type, bool enabled) enableCaptureComponent(type, enabled); } -void APIPCamera::setCaptureUpdate(USceneCaptureComponent2D* capture, bool nodisplay) -{ - capture->bCaptureEveryFrame = !nodisplay; - capture->bCaptureOnMovement = !nodisplay; - capture->bAlwaysPersistRenderingState = true; -} - -void APIPCamera::setCameraTypeUpdate(ImageType type, bool nodisplay) -{ - USceneCaptureComponent2D* capture = getCaptureComponent(type, false); - if (capture != nullptr) - setCaptureUpdate(capture, nodisplay); -} - void APIPCamera::setCameraPose(const msr::airlib::Pose& relative_pose) { FTransform pose = ned_transform_->fromRelativeNed(relative_pose); @@ -608,7 +586,14 @@ void APIPCamera::onViewModeChanged(bool nodisplay) for (unsigned int image_type = 0; image_type < imageTypeCount(); ++image_type) { USceneCaptureComponent2D* capture = getCaptureComponent(static_cast(image_type), false); if (capture) { - setCaptureUpdate(capture, nodisplay); + if (nodisplay) { + capture->bCaptureEveryFrame = false; + capture->bCaptureOnMovement = false; + } + else { + capture->bCaptureEveryFrame = true; + capture->bCaptureOnMovement = true; + } } } } diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index da4b80de5c..b4c6644939 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -67,9 +67,6 @@ class AIRSIM_API APIPCamera : public ACineCameraActor //CinemAirSim void setCameraTypeEnabled(ImageType type, bool enabled); bool getCameraTypeEnabled(ImageType type) const; - void setCaptureUpdate(USceneCaptureComponent2D* capture, bool nodisplay); - void setCameraTypeUpdate(ImageType type, bool nodisplay); - void setupCameraFromSettings(const CameraSetting& camera_setting, const NedTransform& ned_transform); void setCameraPose(const msr::airlib::Pose& relative_pose); void setCameraFoV(float fov_degrees); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 2dd3db74d8..f5c6a71adb 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -22,7 +22,7 @@ PawnSimApi::PawnSimApi(const Params& params) void PawnSimApi::initialize() { Kinematics::State initial_kinematic_state = Kinematics::State::zero(); - + ; initial_kinematic_state.pose = getPose(); kinematics_.reset(new Kinematics(initial_kinematic_state)); @@ -528,13 +528,6 @@ const msr::airlib::Kinematics::State* PawnSimApi::getGroundTruthKinematics() con { return &kinematics_->getState(); } - -void PawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision) -{ - unused(ignore_collision); - - return kinematics_->setState(state); -} const msr::airlib::Environment* PawnSimApi::getGroundTruthEnvironment() const { return environment_.get(); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index 4ec964ca9f..00a51f7bc6 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -37,7 +37,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting; typedef msr::airlib::ImageCaptureBase ImageCaptureBase; typedef msr::airlib::DetectionInfo DetectionInfo; - typedef msr::airlib::Kinematics Kinematics; struct Params { @@ -93,7 +92,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual void updateRenderedState(float dt) override; virtual void updateRendering(float dt) override; virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override; - virtual void setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) override; virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override; virtual std::string getRecordFileLine(bool is_header_line) const override; virtual void reportState(msr::airlib::StateReporter& reporter) override; @@ -151,6 +149,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase private: //vars typedef msr::airlib::AirSimSettings AirSimSettings; + typedef msr::airlib::Kinematics Kinematics; typedef msr::airlib::Environment Environment; Params params_; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index bffda000ff..71000329c0 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -133,11 +133,8 @@ void ASimHUD::updateWidgetSubwindowVisibility() bool is_visible = getSubWindowSettings().at(window_index).visible && camera != nullptr; - if (camera != nullptr) { + if (camera != nullptr) camera->setCameraTypeEnabled(camera_type, is_visible); - //sub-window captures don’t count as a request, set bCaptureEveryFrame and bCaptureOnMovement to display so we can show correctly the subwindow - camera->setCameraTypeUpdate(camera_type, false); - } widget_->setSubwindowVisibility(window_index, is_visible, diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 4e755a2d8e..1001c4c710 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -64,10 +64,6 @@ ASimModeBase::ASimModeBase() } else loading_screen_widget_ = nullptr; - static ConstructorHelpers::FObjectFinder domain_rand_mat_finder(TEXT("Material'/AirSim/HUDAssets/DomainRandomizationMaterial.DomainRandomizationMaterial'")); - if (domain_rand_mat_finder.Succeeded()) { - domain_rand_material_ = domain_rand_mat_finder.Object; - } } void ASimModeBase::toggleLoadingScreen(bool is_visible) @@ -283,12 +279,14 @@ void ASimModeBase::setTimeOfDay(bool is_enabled, const std::string& start_dateti bool ASimModeBase::isPaused() const { - return UGameplayStatics::IsGamePaused(this->GetWorld()); + return false; } void ASimModeBase::pause(bool is_paused) { - UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused); + //should be overridden by derived class + unused(is_paused); + throw std::domain_error("Pause is not implemented by SimMode"); } void ASimModeBase::continueForTime(double seconds) @@ -644,9 +642,7 @@ std::unique_ptr ASimModeBase::createVehicleApi(APawn* vehicle_pawn) bool ASimModeBase::createVehicleAtRuntime(const std::string& vehicle_name, const std::string& vehicle_type, const msr::airlib::Pose& pose, const std::string& pawn_path) { - // Convert to lowercase as done during settings loading - const std::string vehicle_type_lower = Utils::toLower(vehicle_type); - if (!isVehicleTypeSupported(vehicle_type_lower)) { + if (!isVehicleTypeSupported(vehicle_type)) { Utils::log(Utils::stringf("Vehicle type %s is not supported in this game mode", vehicle_type.c_str()), Utils::kLogLevelWarn); return false; } @@ -655,15 +651,21 @@ bool ASimModeBase::createVehicleAtRuntime(const std::string& vehicle_name, const // Maybe allow passing a JSON string for the vehicle settings? // Retroactively adjust AirSimSettings, so it's like we knew about this vehicle all along - AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type_lower, pose, pawn_path); + AirSimSettings::singleton().addVehicleSetting(vehicle_name, vehicle_type, pose, pawn_path); const auto* vehicle_setting = getSettings().getVehicleSetting(vehicle_name); auto spawned_pawn = createVehiclePawn(*vehicle_setting); + auto vehicle_sim_api = createVehicleApi(spawned_pawn); // Usually physics registration happens at init, in ASimModeWorldBase::initializeForPlay(), but not in this case + vehicle_sim_api->reset(); registerPhysicsBody(vehicle_sim_api.get()); + // Can't be done before the vehicle apis have been created + if ((vehicle_setting->is_fpv_vehicle || !getApiProvider()->hasDefaultVehicle()) && vehicle_name != "") + getApiProvider()->makeDefaultVehicle(vehicle_name); + vehicle_sim_apis_.push_back(std::move(vehicle_sim_api)); return true; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 3b867d57a6..756f227d34 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -120,7 +120,6 @@ class AIRSIM_API ASimModeBase : public AActor TMap asset_map; TMap scene_object_map; - UMaterial* domain_rand_material_; protected: //must overrides typedef msr::airlib::AirSimSettings AirSimSettings; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 176cb610f3..445f72061c 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -25,9 +25,6 @@ void ASimModeWorldBase::initializeForPlay() void ASimModeWorldBase::registerPhysicsBody(msr::airlib::VehicleSimApiBase* physicsBody) { - // Reset the vehicle as well before registering it - // Similar to what happens in initializeForPlay() above - physicsBody->reset(); physics_world_.get()->addBody(physicsBody); } @@ -94,7 +91,7 @@ bool ASimModeWorldBase::isPaused() const void ASimModeWorldBase::pause(bool is_paused) { physics_world_->pause(is_paused); - ASimModeBase::pause(is_paused); + UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused); } void ASimModeWorldBase::continueForTime(double seconds) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index d2a7bc4825..22df2cb93b 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp @@ -7,8 +7,8 @@ using namespace msr::airlib; CarPawnSimApi::CarPawnSimApi(const Params& params, - const msr::airlib::CarApiBase::CarControls& keyboard_controls) - : PawnSimApi(params), keyboard_controls_(keyboard_controls) + const msr::airlib::CarApiBase::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement) + : PawnSimApi(params), params_(params), keyboard_controls_(keyboard_controls) { } @@ -16,19 +16,21 @@ void CarPawnSimApi::initialize() { PawnSimApi::initialize(); - //create vehicle params - std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); - - vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), - sensor_factory, - *getGroundTruthKinematics(), - *getGroundTruthEnvironment()); - pawn_api_ = std::unique_ptr(new CarPawnApi(static_cast(getPawn()), getGroundTruthKinematics(), vehicle_api_.get())); + createVehicleApi(static_cast(params_.pawn), params_.home_geopoint); //TODO: should do reset() here? joystick_controls_ = msr::airlib::CarApiBase::CarControls(); } +void CarPawnSimApi::createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint) +{ + //create vehicle params + std::shared_ptr sensor_factory = std::make_shared(getPawn(), &getNedTransform()); + + vehicle_api_ = CarApiFactory::createApi(getVehicleSetting(), sensor_factory, (*getGroundTruthKinematics()), (*getGroundTruthEnvironment()), home_geopoint); + pawn_api_ = std::unique_ptr(new CarPawnApi(pawn, getGroundTruthKinematics(), vehicle_api_.get())); +} + std::string CarPawnSimApi::getRecordFileLine(bool is_header_line) const { std::string common_line = PawnSimApi::getRecordFileLine(is_header_line); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index fe09411a69..21a2fa02b1 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h @@ -28,7 +28,7 @@ class CarPawnSimApi : public PawnSimApi //VehicleSimApiBase interface //implements game interface to update pawn CarPawnSimApi(const Params& params, - const msr::airlib::CarApiBase::CarControls& keyboard_controls); + const msr::airlib::CarApiBase::CarControls& keyboard_controls, UWheeledVehicleMovementComponent* movement); virtual void update() override; virtual void reportState(StateReporter& reporter) override; @@ -52,9 +52,12 @@ class CarPawnSimApi : public PawnSimApi virtual void resetImplementation() override; private: + void createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint& home_geopoint); void updateCarControls(); private: + Params params_; + std::unique_ptr vehicle_api_; std::unique_ptr pawn_api_; std::vector vehicle_api_messages_; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp index 09062ae12a..d6155658eb 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.cpp @@ -25,6 +25,21 @@ void ASimModeCar::initializePauseState() pause(false); } +bool ASimModeCar::isPaused() const +{ + return current_clockspeed_ == 0; +} + +void ASimModeCar::pause(bool is_paused) +{ + if (is_paused) + current_clockspeed_ = 0; + else + current_clockspeed_ = getSettings().clock_speed; + + UAirBlueprintLib::setUnrealClockSpeed(this, current_clockspeed_); +} + void ASimModeCar::continueForTime(double seconds) { pause_period_start_ = ClockFactory::get()->nowNanos(); @@ -123,7 +138,8 @@ std::unique_ptr ASimModeCar::createVehicleSimApi( { auto vehicle_pawn = static_cast(pawn_sim_api_params.pawn); auto vehicle_sim_api = std::unique_ptr(new CarPawnSimApi(pawn_sim_api_params, - vehicle_pawn->getKeyBoardControls())); + vehicle_pawn->getKeyBoardControls(), + vehicle_pawn->getVehicleMovementComponent())); vehicle_sim_api->initialize(); vehicle_sim_api->reset(); return vehicle_sim_api; @@ -133,4 +149,4 @@ msr::airlib::VehicleApiBase* ASimModeCar::getVehicleApi(const PawnSimApi::Params { const auto car_sim_api = static_cast(sim_api); return car_sim_api->getVehicleApi(); -} +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.h b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.h index 4810c31740..905a0a0e7b 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Car/SimModeCar.h @@ -17,6 +17,8 @@ class AIRSIM_API ASimModeCar : public ASimModeBase virtual void BeginPlay() override; virtual void Tick(float DeltaSeconds) override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; virtual void continueForTime(double seconds) override; virtual void continueForFrames(uint32_t frames) override; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp index e3a6e99c9e..b3771cba70 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/ComputerVisionPawn.cpp @@ -40,25 +40,25 @@ void AComputerVisionPawn::initializeForBeginPlay() FActorSpawnParameters camera_spawn_params; camera_spawn_params.SpawnCollisionHandlingOverride = ESpawnActorCollisionHandlingMethod::AdjustIfPossibleButAlwaysSpawn; - camera_spawn_params.Name = FName(*(this->GetName() + "_camera_front_center")); + camera_spawn_params.Name = "camera_front_center"; camera_front_center_ = this->GetWorld()->SpawnActor(pip_camera_class_, camera_transform, camera_spawn_params); camera_front_center_->AttachToComponent(camera_front_center_base_, FAttachmentTransformRules::KeepRelativeTransform); - camera_spawn_params.Name = FName(*(this->GetName() + "_camera_front_left")); + camera_spawn_params.Name = "camera_front_left"; camera_front_left_ = this->GetWorld()->SpawnActor(pip_camera_class_, camera_transform, camera_spawn_params); camera_front_left_->AttachToComponent(camera_front_left_base_, FAttachmentTransformRules::KeepRelativeTransform); - camera_spawn_params.Name = FName(*(this->GetName() + "_camera_front_right")); + camera_spawn_params.Name = "camera_front_right"; camera_front_right_ = this->GetWorld()->SpawnActor(pip_camera_class_, camera_transform, camera_spawn_params); camera_front_right_->AttachToComponent(camera_front_right_base_, FAttachmentTransformRules::KeepRelativeTransform); - camera_spawn_params.Name = FName(*(this->GetName() + "_camera_bottom_center")); + camera_spawn_params.Name = "camera_bottom_center"; camera_bottom_center_ = this->GetWorld()->SpawnActor(pip_camera_class_, FTransform(FRotator(-90, 0, 0), FVector::ZeroVector), camera_spawn_params); camera_bottom_center_->AttachToComponent(camera_bottom_center_base_, FAttachmentTransformRules::KeepRelativeTransform); - camera_spawn_params.Name = FName(*(this->GetName() + "_camera_back_center")); + camera_spawn_params.Name = "camera_back_center"; camera_back_center_ = this->GetWorld()->SpawnActor(pip_camera_class_, FTransform(FRotator(0, -180, 0), FVector::ZeroVector), camera_spawn_params); diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp index 6385c1a4c9..b8b553da59 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.cpp @@ -75,3 +75,13 @@ msr::airlib::VehicleApiBase* ASimModeComputerVision::getVehicleApi(const PawnSim //we don't have real vehicle so no vehicle API return nullptr; } + +bool ASimModeComputerVision::isPaused() const +{ + return UGameplayStatics::IsGamePaused(this->GetWorld()); +} + +void ASimModeComputerVision::pause(bool is_paused) +{ + UGameplayStatics::SetGamePaused(this->GetWorld(), is_paused); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h index 779884fcc5..45b1b20abc 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/ComputerVision/SimModeComputerVision.h @@ -29,4 +29,6 @@ class AIRSIM_API ASimModeComputerVision : public ASimModeBase const PawnSimApi::Params& pawn_sim_api_params) const override; virtual msr::airlib::VehicleApiBase* getVehicleApi(const PawnSimApi::Params& pawn_sim_api_params, const PawnSimApi* sim_api) const override; + virtual bool isPaused() const override; + virtual void pause(bool is_paused) override; }; diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp index 0f27009068..cf0d96fa0e 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp @@ -139,16 +139,6 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision) pending_pose_status_ = PendingPoseStatus::RenderPending; } -void MultirotorPawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision) -{ - multirotor_physics_body_->lock(); - multirotor_physics_body_->updateKinematics(state); - multirotor_physics_body_->setGrounded(false); - multirotor_physics_body_->unlock(); - pending_pose_collisions_ = ignore_collision; - pending_pose_status_ = PendingPoseStatus::RenderPending; -} - //*** Start: UpdatableState implementation ***// void MultirotorPawnSimApi::resetImplementation() { diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h index f7db8a8fda..9b8c351466 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h @@ -43,7 +43,6 @@ class MultirotorPawnSimApi : public PawnSimApi virtual UpdatableObject* getPhysicsBody() override; virtual void setPose(const Pose& pose, bool ignore_collision) override; - virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override; virtual void pawnTick(float dt) override; msr::airlib::MultirotorApiBase* getVehicleApi() const diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index c832051340..bb803b5c09 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -7,11 +7,8 @@ #include "DrawDebugHelpers.h" #include "Runtime/Engine/Classes/Components/LineBatchComponent.h" #include "Runtime/Engine/Classes/Engine/Engine.h" -#include "Misc/OutputDeviceNull.h" -#include "ImageUtils.h" #include #include -#include WorldSimApi::WorldSimApi(ASimModeBase* simmode) : simmode_(simmode) {} @@ -85,80 +82,65 @@ bool WorldSimApi::destroyObject(const std::string& object_name) return result; } -std::vector WorldSimApi::listAssets() const +std::string WorldSimApi::spawnObject(std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled) { - std::vector all_assets; - - for (const TPair& pair : simmode_->asset_map) { - all_assets.push_back(std::string(TCHAR_TO_UTF8(*pair.Key))); - } - - return all_assets; -} - -std::string WorldSimApi::spawnObject(const std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled, bool is_blueprint) -{ - FString asset_name(load_object.c_str()); - FAssetData* load_asset = simmode_->asset_map.Find(asset_name); - - if (!load_asset->IsValid()) { - throw std::invalid_argument("There were no objects with name " + load_object + " found in the Registry"); - } - // Create struct for Location and Rotation of actor in Unreal FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose); - bool spawned_object = false; - std::string final_object_name = object_name; - - UAirBlueprintLib::RunCommandOnGameThread([this, load_asset, &final_object_name, &spawned_object, &actor_transform, &scale, &physics_enabled, &is_blueprint]() { - // Ensure new non-matching name for the object - std::vector matching_names = UAirBlueprintLib::ListMatchingActors(simmode_, ".*" + final_object_name + ".*"); - if (matching_names.size() > 0) { - int greatest_num{ 0 }; - for (const auto& match : matching_names) { - std::string number_extension = match.substr(match.find_last_not_of("0123456789") + 1); - if (number_extension != "") { - greatest_num = std::max(greatest_num, std::stoi(number_extension)); + bool found_object = false, spawned_object = false; + UAirBlueprintLib::RunCommandOnGameThread([this, load_object, &object_name, &actor_transform, &found_object, &spawned_object, &scale, &physics_enabled]() { + FString asset_name = FString(load_object.c_str()); + FAssetData* LoadAsset = simmode_->asset_map.Find(asset_name); + + if (LoadAsset) { + found_object = true; + UStaticMesh* LoadObject = dynamic_cast(LoadAsset->GetAsset()); + std::vector matching_names = UAirBlueprintLib::ListMatchingActors(simmode_->GetWorld(), ".*" + object_name + ".*"); + if (matching_names.size() > 0) { + size_t greatest_num{ 0 }, result{ 0 }; + for (auto match : matching_names) { + std::string number_extension = match.substr(match.find_last_not_of("0123456789") + 1); + if (number_extension != "") { + result = std::stoi(number_extension); + greatest_num = greatest_num > result ? greatest_num : result; + } } + object_name += std::to_string(greatest_num + 1); + } + FActorSpawnParameters new_actor_spawn_params; + new_actor_spawn_params.Name = FName(object_name.c_str()); + //new_actor_spawn_params.NameMode = FActorSpawnParameters::ESpawnActorNameMode::Required_ReturnNull; + AActor* NewActor = this->createNewActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + + if (NewActor) { + spawned_object = true; + simmode_->scene_object_map.Add(FString(object_name.c_str()), NewActor); } - final_object_name += std::to_string(greatest_num + 1); - } - - FActorSpawnParameters new_actor_spawn_params; - new_actor_spawn_params.Name = FName(final_object_name.c_str()); - AActor* NewActor; - if (is_blueprint) { - UBlueprint* LoadObject = Cast(load_asset->GetAsset()); - NewActor = this->createNewBPActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + UAirBlueprintLib::setSimulatePhysics(NewActor, physics_enabled); } else { - UStaticMesh* LoadObject = dynamic_cast(load_asset->GetAsset()); - NewActor = this->createNewStaticMeshActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + found_object = false; } - - if (IsValid(NewActor)) { - spawned_object = true; - simmode_->scene_object_map.Add(FString(final_object_name.c_str()), NewActor); - } - - UAirBlueprintLib::setSimulatePhysics(NewActor, physics_enabled); }, true); + if (!found_object) { + throw std::invalid_argument( + "There were no objects with name " + load_object + " found in the Registry"); + } if (!spawned_object) { throw std::invalid_argument( "Engine could not spawn " + load_object + " because of a stale reference of same name"); } - return final_object_name; + return object_name; } -AActor* WorldSimApi::createNewStaticMeshActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh) +AActor* WorldSimApi::createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh) { AActor* NewActor = simmode_->GetWorld()->SpawnActor(AActor::StaticClass(), FVector::ZeroVector, FRotator::ZeroRotator, spawn_params); - if (IsValid(NewActor)) { + if (NewActor) { UStaticMeshComponent* ObjectComponent = NewObject(NewActor); ObjectComponent->SetStaticMesh(static_mesh); ObjectComponent->SetRelativeLocation(FVector(0, 0, 0)); @@ -171,33 +153,6 @@ AActor* WorldSimApi::createNewStaticMeshActor(const FActorSpawnParameters& spawn return NewActor; } -AActor* WorldSimApi::createNewBPActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UBlueprint* blueprint) -{ - UClass* new_bp = static_cast(blueprint->GeneratedClass); - AActor* new_actor = simmode_->GetWorld()->SpawnActor(new_bp, FVector::ZeroVector, FRotator::ZeroRotator, spawn_params); - - if (new_actor) { - new_actor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics); - } - return new_actor; -} - -bool WorldSimApi::setLightIntensity(const std::string& light_name, float intensity) -{ - bool result = false; - UAirBlueprintLib::RunCommandOnGameThread([this, &light_name, &intensity, &result]() { - AActor* light_actor = simmode_->scene_object_map.FindRef(FString(light_name.c_str())); - - if (light_actor) { - const FString command = FString::Printf(TEXT("SetIntensity %f"), intensity); - FOutputDeviceNull ar; - result = light_actor->CallFunctionByNameWithArguments(*command, ar, nullptr, true); - } - }, - true); - return result; -} - bool WorldSimApi::createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) { bool success = false; @@ -476,74 +431,6 @@ std::unique_ptr> WorldSimApi::swapTextures(const std::s return swappedObjectNames; } -bool WorldSimApi::setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) -{ - bool success = false; - UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &texture_path, &success]() { - if (!IsValid(simmode_->domain_rand_material_)) { - UAirBlueprintLib::LogMessageString("Cannot find material for domain randomization", - "", - LogDebugLevel::Failure); - } - else { - UTexture2D* texture_desired = FImageUtils::ImportFileAsTexture2D(FString(texture_path.c_str())); - AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); - - if (IsValid(actor)) { - TArray components; - actor->GetComponents(components); - for (UStaticMeshComponent* staticMeshComponent : components) { - UMaterialInstanceDynamic* dynamic_material = UMaterialInstanceDynamic::Create(simmode_->domain_rand_material_, staticMeshComponent); - dynamic_material->SetTextureParameterValue("TextureParameter", texture_desired); - staticMeshComponent->SetMaterial(0, dynamic_material); - } - success = true; - } - else { - UAirBlueprintLib::LogMessageString("Cannot find specified actor for domain randomization", - "", - LogDebugLevel::Failure); - } - } - }, - true); - - return success; -} - -bool WorldSimApi::setObjectMaterial(const std::string& object_name, const std::string& material_name) -{ - bool success = false; - UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &material_name, &success]() { - AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); - UMaterial* material = static_cast(StaticLoadObject(UMaterial::StaticClass(), nullptr, *FString(material_name.c_str()))); - - if (!IsValid(material)) { - UAirBlueprintLib::LogMessageString("Cannot find specified material for domain randomization", - "", - LogDebugLevel::Failure); - } - else { - if (IsValid(actor)) { - TArray components; - actor->GetComponents(components); - for (UStaticMeshComponent* staticMeshComponent : components) { - staticMeshComponent->SetMaterial(0, material); - } - success = true; - } - else { - UAirBlueprintLib::LogMessageString("Cannot find specified actor for domain randomization", - "", - LogDebugLevel::Failure); - } - } - }, - true); - - return success; -} - //----------- Plotting APIs ----------/ void WorldSimApi::simFlushPersistentMarkers() { diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index 7174d89459..9def1dd719 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -24,9 +24,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual bool loadLevel(const std::string& level_name) override; - virtual std::string spawnObject(const std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled, bool is_blueprint) override; + virtual std::string spawnObject(std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale, bool physics_enabled) override; virtual bool destroyObject(const std::string& object_name) override; - virtual std::vector listAssets() const override; virtual bool isPaused() const override; virtual void reset() override; @@ -48,10 +47,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual bool setLightIntensity(const std::string& light_name, float intensity) override; virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; - virtual bool setObjectMaterial(const std::string& object_name, const std::string& material_name) override; - virtual bool setObjectMaterialFromTexture(const std::string& object_name, const std::string& texture_path) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; @@ -120,8 +116,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::vector getDetections(ImageCaptureBase::ImageType image_type, const CameraDetails& camera_details) override; private: - AActor* createNewStaticMeshActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); - AActor* createNewBPActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UBlueprint* blueprint); + AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); void spawnPlayer(); private: diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index 11d0c9bcb2..c966e74cc2 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -25,7 +25,7 @@ endmacro(SetupConsoleBuild) macro(CommonSetup) find_package(Threads REQUIRED) - find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../.." REQUIRED) + find_path(AIRSIM_ROOT NAMES AirSim.sln PATHS ".." "../.." "../../.." "../../../.." "../../../../.." "../../../../../.." REQUIRED) #setup output paths set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/output/lib) @@ -44,15 +44,13 @@ macro(CommonSetup) IF(UNIX) set(RPC_LIB_DEFINES "-D MSGPACK_PP_VARIADICS_MSVC=0") set(BUILD_TYPE "linux") - set(CMAKE_CXX_STANDARD 17) - if (APPLE) + set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wstrict-aliasing -D__CLANG__") else () set(CMAKE_CXX_FLAGS "\ - -Wall -Wextra \ - -Wnon-virtual-dtor -Woverloaded-virtual \ - -Wno-variadic-macros -Wno-unused-function -Wno-unused \ + -std=c++17 -ggdb -Wall -Wextra \ + -Wno-variadic-macros -Wno-parentheses -Wno-unused-function -Wno-unused \ -pthread \ ${RPC_LIB_DEFINES} ${CMAKE_CXX_FLAGS}") @@ -98,3 +96,4 @@ macro(CommonSetup) endif() endmacro(CommonSetup) + diff --git a/docs/apis.md b/docs/apis.md index 8a9f56dc75..422e7802cd 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -217,19 +217,6 @@ AirSim offers API to retrieve point cloud data from Lidar sensors on vehicles. Y More on [lidar APIs and settings](lidar.md) and [sensor settings](sensors.md) -### Light Control APIs - -Lights that can be manipulated inside AirSim can be created via the `simSpawnObject()` API by passing either `PointLightBP` or `SpotLightBP` as the `asset_name` parameter and `True` as the `is_blueprint` parameter. Once a light has been spawned, it can be manipulated using the following API: - -* `simSetLightIntensity`: This allows you to edit a light's intensity or brightness. It takes two parameters, `light_name`, the name of the light object returned by a previous call to `simSpawnObject()`, and `intensity`, a float value. - -### Texture APIs - -Textures can be dynamically set on objects via these APIs: - -* `simSetObjectMaterial`: This sets an object's material using an existing Unreal material asset. It takes two string parameters, `object_name` and `material_name`. -* `simSetObjectMaterialFromTexture`: This sets an object's material using a path to a texture. It takes two string parameters, `object_name` and `texture_path`. - ### Multiple Vehicles AirSim supports multiple vehicles and control them through APIs. Please [Multiple Vehicles](multi_vehicle.md) doc. diff --git a/docs/faq.md b/docs/faq.md index d061426349..4736dcf57f 100644 --- a/docs/faq.md +++ b/docs/faq.md @@ -27,7 +27,7 @@ ###### Unreal editor is slow when it is not the active window ->Go to Edit/Editor Preferences, select "All Settings" and type "CPU" in the search box. +>Go to Edit/Editor Preferences, select "All Settings" and type "CPU" in the search box. >It should find the setting titled "Use Less CPU when in Background", and you want to uncheck this checkbox. @@ -40,7 +40,7 @@ ###### Where is the setting file and how do I modify it? ->AirSim will create empty settings file at `~/Documents/AirSim/settings.json`. You can view the available [settings options](settings.md). +>AirSim will create empty settings file at `~/Documents/AirSim/settings.json`. You can view the available [settings options](settings.md). @@ -110,8 +110,8 @@ ###### What computer do you need? ->It depends on how big your Unreal Environment is. The Blocks environment that comes with AirSim is very basic and works on typical laptops. The ->[Modular Neighborhood Pack](https://www.unrealengine.com/marketplace/modular-neighborhood-pack) that we use ourselves for research requires GPUs with at least 4GB of RAM. The +>It depends on how big your Unreal Environment is. The Blocks environment that comes with AirSim is very basic and works on typical laptops. The +>[Modular Neighborhood Pack](https://www.unrealengine.com/marketplace/modular-neighborhood-pack) that we use ourselves for research requires GPUs with at least 4GB of RAM. The >[Open World environment](https://www.unrealengine.com/marketplace/open-world-demo-collection) needs GPU with 8GB RAM. Our typical development machines have 32GB of RAM and >NVIDIA TitanX and a [fast hard drive](hard_drive.md). @@ -147,4 +147,4 @@ * [Remote Control FAQ](remote_control.md#faq) * [Unreal Blocks Environment FAQ](unreal_blocks.md#faq) * [Unreal Custom Environment FAQ](unreal_custenv.md#faq) -* [Packaging AirSim](build_faq.md#packaging-a-binary-including-the-airsim-plugin) +* [Packaging AirSim](package_binaries.md) diff --git a/docs/image_apis.md b/docs/image_apis.md index 963f24702e..cf54a16b18 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -217,9 +217,7 @@ To change resolution, FOV etc, you can use [settings.json](settings.md). For exa DisparityNormalized = 4, Segmentation = 5, SurfaceNormals = 6, - Infrared = 7, - OpticalFlow = 8, - OpticalFlowVis = 9 + Infrared = 7 ``` ### DepthPlanar and DepthPerspective @@ -289,8 +287,5 @@ The `simGetSegmentationObjectID` API allows you get object ID for given mesh nam ### Infrared Currently this is just a map from object ID to grey scale 0-255. So any mesh with object ID 42 shows up with color (42, 42, 42). Please see [segmentation section](#segmentation) for more details on how to set object IDs. Typically noise setting can be applied for this image type to get slightly more realistic effect. We are still working on adding other infrared artifacts and any contributions are welcome. -### OpticalFlow and OpticalFlowVis -These image types return information about motion perceived by the point of view of the camera. OpticalFlow returns a 2-channel image where the channels correspond to vx and vy respectively. OpticalFlowVis is similar to OpticalFlow but converts flow data to RGB for a more 'visual' output. - ## Example Code A complete example of setting vehicle positions at random locations and orientations and then taking images can be found in [GenerateImageGenerator.hpp](https://github.com/Microsoft/AirSim/tree/master/Examples/DataCollection/StereoImageGenerator.hpp). This example generates specified number of stereo images and ground truth disparity image and saving it to [pfm format](pfm.md). diff --git a/docs/images/HelloSpawnedDrones.gif b/docs/images/HelloSpawnedDrones.gif deleted file mode 100644 index 7b1fb8a19b..0000000000 Binary files a/docs/images/HelloSpawnedDrones.gif and /dev/null differ diff --git a/docs/images/simAddVehicle_Car.gif b/docs/images/simAddVehicle_Car.gif deleted file mode 100644 index 426ccf5eca..0000000000 Binary files a/docs/images/simAddVehicle_Car.gif and /dev/null differ diff --git a/docs/multi_vehicle.md b/docs/multi_vehicle.md index deac7ae5ef..ce02b65079 100644 --- a/docs/multi_vehicle.md +++ b/docs/multi_vehicle.md @@ -3,16 +3,14 @@ Since release 1.2, AirSim is fully enabled for multiple vehicles. This capability allows you to create multiple vehicles easily and use APIs to control them. ## Creating Multiple Vehicles - It's as easy as specifying them in [settings.json](settings.md). The `Vehicles` element allows you to specify list of vehicles you want to create along with their initial positions and orientations. The positions are specified in NED coordinates in SI units with origin set at Player Start component in Unreal environment. The orientation is specified as Yaw, Pitch and Roll in degrees. ### Creating Multiple Cars - ```json { "SettingsVersion": 1.2, "SimMode": "Car", - + "Vehicles": { "Car1": { "VehicleType": "PhysXCar", @@ -28,12 +26,11 @@ It's as easy as specifying them in [settings.json](settings.md). The `Vehicles` ``` ### Creating Multiple Drones - ```json { "SettingsVersion": 1.2, "SimMode": "Multirotor", - + "Vehicles": { "Drone1": { "VehicleType": "SimpleFlight", @@ -50,44 +47,18 @@ It's as easy as specifying them in [settings.json](settings.md). The `Vehicles` ``` ## Using APIs for Multiple Vehicles +The new APIs since AirSim 1.2 allows you to specify `vehicle_name`. This name corresponds to keys in json settings (for example, Car1 or Drone2 above). -The new APIs since AirSim 1.2 allows you to specify `vehicle_name`. This name corresponds to keys in json settings (for example, Car1 or Drone2 above). +[Example code for cars](https://github.com/Microsoft/AirSim/tree/master/PythonClient//car/multi_agent_car.py) -[Example code for cars](https://github.com/microsoft/AirSim/blob/master/PythonClient/car/multi_agent_car.py) - -[Example code for multirotors](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/multi_agent_drone.py) +[Example code for multirotors](https://github.com/Microsoft/AirSim/tree/master/PythonClient//multirotor/multi_agent_drone.py) Using APIs for multi-vehicles requires specifying the `vehicle_name`, which needs to be hardcoded in the script or requires parsing of the settings file. There's also a simple API `listVehicles()` which returns a list (vector in C++) of strings containing names of the current vehicles. For example, with the above settings for 2 Cars - -```python +``` >>> client.listVehicles() ['Car1', 'Car2'] ``` ### Demo - [![AirSimMultiple Vehicles Demo Video](images/demo_multi_vehicles.png)](https://youtu.be/35dgcuLuF5M) - -### Creating vehicles at runtime through API - -In the latest master of AirSim, the `simAddVehicle` API can be used to create vehicles at runtime. This is useful to create many such vehicles without needing to specify them in the settings. There are some limitations of this currently, described below - - -`simAddVehicle` takes in the following arguments: - -- `vehicle_name`: Name of the vehicle to be created, this should be unique for each vehicle including any exisiting ones defined in the settings.json -- `vehicle_type`: Type of vehicle, e.g. "simpleflight". Currently only SimpleFlight, PhysXCar, ComputerVision are supported, in their respective SimModes. - Other vehicle types including PX4 and ArduPilot-related aren't supported -- `pose`: Initial pose of the vehicle -- `pawn_path`: Vehicle blueprint path, default empty wbich uses the default blueprint for the vehicle type - -Returns: `bool` Whether vehicle was created - -The usual APIs can be used to control and interact with the vehicle once created, with the `vehicle_name` parameter. Specifying other settings such as additional cameras, etc. isn't possible currently, a future enhancement could be passing JSON string of settings for the vehicle. It also works with the `listVehicles()` API described above, so the vehicles spawned would be included in the list. - -For some examples, check out [HelloSpawnedDrones.cpp](https://github.com/microsoft/AirSim/blob/master/HelloSpawnedDrones/HelloSpawnedDrones.cpp) - - -![HelloSpawnedDrones](images/HelloSpawnedDrones.gif) - -And [runtime_car.py](https://github.com/microsoft/AirSim/tree/master/PythonClient/car/runtime_car.py) - - -![runtime_car](images/simAddVehicle_Car.gif) diff --git a/docs/retexturing.md b/docs/retexturing.md index 9312aeb41d..7c24f02399 100644 --- a/docs/retexturing.md +++ b/docs/retexturing.md @@ -67,6 +67,4 @@ Results: ![Demo](images/tex_swap_demo.gif) -Note that in this example, different textures were chosen on each actor for the same index value. - -You can also use the `simSetObjectMaterial` and `simSetObjectMaterialFromTexture` APIs to set an object's material to any material asset or filepath of a texture. For more information on using these APIs, see [Texture APIs](apis.md#texture-apis). +Note that in this example, different textures were chosen on each actor for the same index value. \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/CMakeLists.txt b/ros/src/airsim_ros_pkgs/CMakeLists.txt index 0acab7357d..51d69c6b75 100644 --- a/ros/src/airsim_ros_pkgs/CMakeLists.txt +++ b/ros/src/airsim_ros_pkgs/CMakeLists.txt @@ -8,12 +8,11 @@ add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper) add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib) add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wpedantic -Wstrict-null-sentinel -Wno-unused") +set(CMAKE_CXX_STANDARD 11) set(CXX_EXP_LIB "-nostdinc++ -I/usr/include/c++/8 -I/usr/include/x86_64-linux-gnu/c++/8 -nodefaultlibs -l/usr/lib/x86_64-linux-gnu/libc++.so -l/usr/lib/x86_64-linux-gnu/libc++abi.so -lm -lc -lgcc_s -lgcc --lstdc++fs -fmax-errors=10") +-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel") set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/rpclib-2.3.0/include") set(RPC_LIB rpc) # name of .a file with lib prefix diff --git a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h index c727f9d420..6260b89db1 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -300,16 +300,7 @@ class AirsimROSWrapper ros::Time airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const; ros::Time chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const; - // Utility methods to convert airsim_client_ - msr::airlib::MultirotorRpcLibClient* get_multirotor_client(); - msr::airlib::CarRpcLibClient* get_car_client(); - private: - ros::NodeHandle nh_; - ros::NodeHandle nh_private_; - - std::string host_ip_; - // subscriber / services for ALL robots ros::Subscriber vel_cmd_all_body_frame_sub_; ros::Subscriber vel_cmd_all_world_frame_sub_; @@ -335,11 +326,15 @@ class AirsimROSWrapper bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB + std::string host_ip_; std::unique_ptr airsim_client_ = nullptr; // seperate busy connections to airsim, update in their own thread msr::airlib::RpcLibClientBase airsim_client_images_; msr::airlib::RpcLibClientBase airsim_client_lidar_; + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS? ros::CallbackQueue img_timer_cb_queue_; @@ -395,4 +390,4 @@ class AirsimROSWrapper static constexpr char R_YML_NAME[] = "rectification_matrix"; static constexpr char P_YML_NAME[] = "projection_matrix"; static constexpr char DMODEL_YML_NAME[] = "distortion_model"; -}; +}; \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/include/airsim_settings_parser.h b/ros/src/airsim_ros_pkgs/include/airsim_settings_parser.h index 1c89297b3f..c5a131f464 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_settings_parser.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_settings_parser.h @@ -20,6 +20,7 @@ class AirSimSettingsParser public: AirSimSettingsParser(const std::string& host_ip); + ~AirSimSettingsParser(){}; bool success(); @@ -31,4 +32,4 @@ class AirSimSettingsParser bool success_; std::string settings_text_; std::string host_ip_; -}; +}; \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/geodetic_conv.hpp b/ros/src/airsim_ros_pkgs/include/geodetic_conv.hpp old mode 100755 new mode 100644 similarity index 100% rename from ros2/src/airsim_ros_pkgs/include/geodetic_conv.hpp rename to ros/src/airsim_ros_pkgs/include/geodetic_conv.hpp diff --git a/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h b/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h index 2e2193d51e..0c57ca930e 100644 --- a/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h +++ b/ros/src/airsim_ros_pkgs/include/pd_position_controller_simple.h @@ -11,7 +11,6 @@ STRICT_MODE_OFF //todo what does this do? #include "common/common_utils/FileSystem.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" -#include "common/GeodeticConverter.hpp" #include #include @@ -23,6 +22,7 @@ STRICT_MODE_OFF //todo what does this do? #include #include #include +#include #include #include @@ -101,8 +101,8 @@ class PIDPositionController void check_reached_goal(); private: - GeodeticConverter geodetic_converter_; - static constexpr bool use_eth_lib_for_geodetic_conv_ = true; + geodetic_converter::GeodeticConverter geodetic_converter_; + bool use_eth_lib_for_geodetic_conv_; ros::NodeHandle nh_; ros::NodeHandle nh_private_; @@ -136,4 +136,4 @@ class PIDPositionController ros::Timer update_control_cmd_timer_; }; -#endif /* _PID_POSITION_CONTROLLER_SIMPLE_ */ +#endif /* _PID_POSITION_CONTROLLER_SIMPLE_ */ \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/scripts/car_joy b/ros/src/airsim_ros_pkgs/scripts/car_joy similarity index 97% rename from ros2/src/airsim_ros_pkgs/scripts/car_joy rename to ros/src/airsim_ros_pkgs/scripts/car_joy index e7b3f42c79..b8fab4ef48 100755 --- a/ros2/src/airsim_ros_pkgs/scripts/car_joy +++ b/ros/src/airsim_ros_pkgs/scripts/car_joy @@ -1,150 +1,150 @@ -#!/usr/bin/env python - -#capture joystick events using ROS and convert to AirSim Car API commands -#to enable: -# rosrun joy joy_node - -import rospy -import threading -import sensor_msgs -import sensor_msgs.msg -import airsim_ros_pkgs as air -import airsim_ros_pkgs.msg - -class CarCommandTranslator(object): - def __init__(self): - self.lock = threading.Lock() - - self.last_forward_btn = 0 - self.last_reverse_btn = 0 - self.last_nuetral_btn = 0 - self.last_park_btn = 0 - self.last_shift_down_btn = 0 - self.last_shift_up_btn = 0 - self.parked = True - self.last_gear = 0 - self.shift_mode_manual = True - - update_rate_hz = rospy.get_param('~update_rate_hz', 20.0) - self.max_curvature = rospy.get_param('~max_curvature', 0.75) - self.steer_sign = rospy.get_param('~steer_sign', -1) - self.throttle_brake_sign = rospy.get_param('~throttle_brake_sign', 1) - self.auto_gear_max = rospy.get_param('~auto_gear_max', 5) - self.manual_transmission = rospy.get_param('~manual_transmission', True) - self.forward_btn_index = rospy.get_param('~forward_button_index', 0) - self.reverse_btn_index = rospy.get_param('~reverse_button_index', 1) - self.nuetral_btn_index = rospy.get_param('~nuetral_button_index', 2) - self.park_btn_index = rospy.get_param('~park_button_index', 3) - self.shift_down_btn_index = rospy.get_param('~shift_down_index', 4) - self.shift_up_btn_index = rospy.get_param('~shift_up_index', 5) - car_control_topic = rospy.get_param('~car_control_topic', '/airsim_node/drone_1/car_cmd') - - self.joy_msg = None - - self.joy_sub = rospy.Subscriber( - 'joy', - sensor_msgs.msg.Joy, - self.handle_joy) - - self.command_pub = rospy.Publisher( - car_control_topic, - air.msg.CarControls, - queue_size=0 - ) - - self.update_time = rospy.Timer( - rospy.Duration(1.0/update_rate_hz), - self.handle_update_timer - ) - - def handle_joy(self, msg): - with self.lock: - self.joy_msg = msg - - def handle_update_timer(self, ignored): - joy = None - with self.lock: - joy = self.joy_msg - - if joy is None: - return - - controls = airsim_ros_pkgs.msg.CarControls() - - controls.steering = self.steer_sign * self.max_curvature * joy.axes[2] - u = joy.axes[1] * self.throttle_brake_sign - if u > 0.0: - controls.throttle = abs(u) - controls.brake = 0.0 - else: - controls.throttle = 0.0 - controls.brake = abs(u) - - forward_btn = joy.buttons[self.forward_btn_index] - reverse_btn = joy.buttons[self.reverse_btn_index] - nuetral_btn = joy.buttons[self.nuetral_btn_index] - park_btn = joy.buttons[self.park_btn_index] - shift_up_btn = joy.buttons[self.shift_up_btn_index] - shift_down_btn = joy.buttons[self.shift_down_btn_index] - - - # gearing: -1 reverse, 0 N, >= 1 drive - controls.manual = True #set to False for automatic transmission along with manual_gear > 1 - if not self.last_nuetral_btn and nuetral_btn: - self.last_gear = 0 - self.parked = False - controls.manual = True - elif not self.last_forward_btn and forward_btn: - if self.manual_transmission: - self.last_gear = 1 - self.shift_mode_manual = True - else: - self.shift_mode_manual = False - self.last_gear = self.auto_gear_max - - self.parked = False - elif not self.last_reverse_btn and reverse_btn: - self.last_gear = -1 - self.parked = False - self.shift_mode_manual = True - elif not self.last_park_btn and park_btn: - self.parked = True - elif not self.last_shift_down_btn and shift_down_btn and self.last_gear > 1 and self.manual_transmission: - self.last_gear-=1 - self.parked = False - self.shift_mode_manual = True - elif not self.last_shift_up_btn and shift_up_btn and self.last_gear >= 1 and self.manual_transmission: - self.last_gear+=1 - self.parked = False - self.shift_mode_manual = True - - if self.parked: - self.last_gear = 0 - self.shift_mode_manual = True - controls.handbrake = True - else: - controls.handbrake = False - - controls.manual_gear = self.last_gear - controls.manual = self.shift_mode_manual - - now = rospy.Time.now() - controls.header.stamp = now - controls.gear_immediate = True - - self.last_nuetral_btn = nuetral_btn - self.last_forward_btn = forward_btn - self.last_reverse_btn = reverse_btn - self.last_park_btn = park_btn - self.last_shift_down_btn = shift_down_btn - self.last_shift_up_btn = shift_up_btn - - self.command_pub.publish(controls) - - def run(self): - rospy.spin() - -if __name__ == '__main__': - rospy.init_node('car_joy') - node = CarCommandTranslator() +#!/usr/bin/env python + +#capture joystick events using ROS and convert to AirSim Car API commands +#to enable: +# rosrun joy joy_node + +import rospy +import threading +import sensor_msgs +import sensor_msgs.msg +import airsim_ros_pkgs as air +import airsim_ros_pkgs.msg + +class CarCommandTranslator(object): + def __init__(self): + self.lock = threading.Lock() + + self.last_forward_btn = 0 + self.last_reverse_btn = 0 + self.last_nuetral_btn = 0 + self.last_park_btn = 0 + self.last_shift_down_btn = 0 + self.last_shift_up_btn = 0 + self.parked = True + self.last_gear = 0 + self.shift_mode_manual = True + + update_rate_hz = rospy.get_param('~update_rate_hz', 20.0) + self.max_curvature = rospy.get_param('~max_curvature', 0.75) + self.steer_sign = rospy.get_param('~steer_sign', -1) + self.throttle_brake_sign = rospy.get_param('~throttle_brake_sign', 1) + self.auto_gear_max = rospy.get_param('~auto_gear_max', 5) + self.manual_transmission = rospy.get_param('~manual_transmission', True) + self.forward_btn_index = rospy.get_param('~forward_button_index', 0) + self.reverse_btn_index = rospy.get_param('~reverse_button_index', 1) + self.nuetral_btn_index = rospy.get_param('~nuetral_button_index', 2) + self.park_btn_index = rospy.get_param('~park_button_index', 3) + self.shift_down_btn_index = rospy.get_param('~shift_down_index', 4) + self.shift_up_btn_index = rospy.get_param('~shift_up_index', 5) + car_control_topic = rospy.get_param('~car_control_topic', '/airsim_node/drone_1/car_cmd') + + self.joy_msg = None + + self.joy_sub = rospy.Subscriber( + 'joy', + sensor_msgs.msg.Joy, + self.handle_joy) + + self.command_pub = rospy.Publisher( + car_control_topic, + air.msg.CarControls, + queue_size=0 + ) + + self.update_time = rospy.Timer( + rospy.Duration(1.0/update_rate_hz), + self.handle_update_timer + ) + + def handle_joy(self, msg): + with self.lock: + self.joy_msg = msg + + def handle_update_timer(self, ignored): + joy = None + with self.lock: + joy = self.joy_msg + + if joy is None: + return + + controls = airsim_ros_pkgs.msg.CarControls() + + controls.steering = self.steer_sign * self.max_curvature * joy.axes[2] + u = joy.axes[1] * self.throttle_brake_sign + if u > 0.0: + controls.throttle = abs(u) + controls.brake = 0.0 + else: + controls.throttle = 0.0 + controls.brake = abs(u) + + forward_btn = joy.buttons[self.forward_btn_index] + reverse_btn = joy.buttons[self.reverse_btn_index] + nuetral_btn = joy.buttons[self.nuetral_btn_index] + park_btn = joy.buttons[self.park_btn_index] + shift_up_btn = joy.buttons[self.shift_up_btn_index] + shift_down_btn = joy.buttons[self.shift_down_btn_index] + + + # gearing: -1 reverse, 0 N, >= 1 drive + controls.manual = True #set to False for automatic transmission along with manual_gear > 1 + if not self.last_nuetral_btn and nuetral_btn: + self.last_gear = 0 + self.parked = False + controls.manual = True + elif not self.last_forward_btn and forward_btn: + if self.manual_transmission: + self.last_gear = 1 + self.shift_mode_manual = True + else: + self.shift_mode_manual = False + self.last_gear = self.auto_gear_max + + self.parked = False + elif not self.last_reverse_btn and reverse_btn: + self.last_gear = -1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_park_btn and park_btn: + self.parked = True + elif not self.last_shift_down_btn and shift_down_btn and self.last_gear > 1 and self.manual_transmission: + self.last_gear-=1 + self.parked = False + self.shift_mode_manual = True + elif not self.last_shift_up_btn and shift_up_btn and self.last_gear >= 1 and self.manual_transmission: + self.last_gear+=1 + self.parked = False + self.shift_mode_manual = True + + if self.parked: + self.last_gear = 0 + self.shift_mode_manual = True + controls.handbrake = True + else: + controls.handbrake = False + + controls.manual_gear = self.last_gear + controls.manual = self.shift_mode_manual + + now = rospy.Time.now() + controls.header.stamp = now + controls.gear_immediate = True + + self.last_nuetral_btn = nuetral_btn + self.last_forward_btn = forward_btn + self.last_reverse_btn = reverse_btn + self.last_park_btn = park_btn + self.last_shift_down_btn = shift_down_btn + self.last_shift_up_btn = shift_up_btn + + self.command_pub.publish(controls) + + def run(self): + rospy.spin() + +if __name__ == '__main__': + rospy.init_node('car_joy') + node = CarCommandTranslator() node.run() \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/scripts/car_joy.py b/ros/src/airsim_ros_pkgs/scripts/car_joy.py deleted file mode 100755 index 448b76d2e4..0000000000 --- a/ros/src/airsim_ros_pkgs/scripts/car_joy.py +++ /dev/null @@ -1,151 +0,0 @@ -#!/usr/bin/env python - -#capture joystick events using ROS and convert to AirSim Car API commands -#to enable: -# rosrun joy joy_node - -import rospy -import threading -import sensor_msgs -import sensor_msgs.msg -import airsim_ros_pkgs as air -import airsim_ros_pkgs.msg - -class CarCommandTranslator(object): - def __init__(self): - self.lock = threading.Lock() - - self.last_forward_btn = 0 - self.last_reverse_btn = 0 - self.last_neutral_btn = 0 - self.last_park_btn = 0 - self.last_shift_down_btn = 0 - self.last_shift_up_btn = 0 - self.parked = True - self.last_gear = 0 - self.shift_mode_manual = True - - update_rate_hz = rospy.get_param('~update_rate_hz', 20.0) - self.max_curvature = rospy.get_param('~max_curvature', 0.75) - self.steer_sign = rospy.get_param('~steer_sign', -1) - self.throttle_brake_sign = rospy.get_param('~throttle_brake_sign', 1) - self.auto_gear_max = rospy.get_param('~auto_gear_max', 5) - self.manual_transmission = rospy.get_param('~manual_transmission', True) - self.forward_btn_index = rospy.get_param('~forward_button_index', 0) - self.reverse_btn_index = rospy.get_param('~reverse_button_index', 1) - # Below was an earlier typo, written like this for compatibility - self.neutral_btn_index = rospy.get_param('~neutral_button_index', rospy.get_param('~nuetral_button_index', 2)) - self.park_btn_index = rospy.get_param('~park_button_index', 3) - self.shift_down_btn_index = rospy.get_param('~shift_down_index', 4) - self.shift_up_btn_index = rospy.get_param('~shift_up_index', 5) - car_control_topic = rospy.get_param('~car_control_topic', '/airsim_node/drone_1/car_cmd') - - self.joy_msg = None - - self.joy_sub = rospy.Subscriber( - 'joy', - sensor_msgs.msg.Joy, - self.handle_joy) - - self.command_pub = rospy.Publisher( - car_control_topic, - air.msg.CarControls, - queue_size=0 - ) - - self.update_time = rospy.Timer( - rospy.Duration(1.0/update_rate_hz), - self.handle_update_timer - ) - - def handle_joy(self, msg): - with self.lock: - self.joy_msg = msg - - def handle_update_timer(self, ignored): - joy = None - with self.lock: - joy = self.joy_msg - - if joy is None: - return - - controls = airsim_ros_pkgs.msg.CarControls() - - controls.steering = self.steer_sign * self.max_curvature * joy.axes[2] - u = joy.axes[1] * self.throttle_brake_sign - if u > 0.0: - controls.throttle = abs(u) - controls.brake = 0.0 - else: - controls.throttle = 0.0 - controls.brake = abs(u) - - forward_btn = joy.buttons[self.forward_btn_index] - reverse_btn = joy.buttons[self.reverse_btn_index] - neutral_btn = joy.buttons[self.neutral_btn_index] - park_btn = joy.buttons[self.park_btn_index] - shift_up_btn = joy.buttons[self.shift_up_btn_index] - shift_down_btn = joy.buttons[self.shift_down_btn_index] - - - # gearing: -1 reverse, 0 N, >= 1 drive - controls.manual = True #set to False for automatic transmission along with manual_gear > 1 - if not self.last_neutral_btn and neutral_btn: - self.last_gear = 0 - self.parked = False - controls.manual = True - elif not self.last_forward_btn and forward_btn: - if self.manual_transmission: - self.last_gear = 1 - self.shift_mode_manual = True - else: - self.shift_mode_manual = False - self.last_gear = self.auto_gear_max - - self.parked = False - elif not self.last_reverse_btn and reverse_btn: - self.last_gear = -1 - self.parked = False - self.shift_mode_manual = True - elif not self.last_park_btn and park_btn: - self.parked = True - elif not self.last_shift_down_btn and shift_down_btn and self.last_gear > 1 and self.manual_transmission: - self.last_gear-=1 - self.parked = False - self.shift_mode_manual = True - elif not self.last_shift_up_btn and shift_up_btn and self.last_gear >= 1 and self.manual_transmission: - self.last_gear+=1 - self.parked = False - self.shift_mode_manual = True - - if self.parked: - self.last_gear = 0 - self.shift_mode_manual = True - controls.handbrake = True - else: - controls.handbrake = False - - controls.manual_gear = self.last_gear - controls.manual = self.shift_mode_manual - - now = rospy.Time.now() - controls.header.stamp = now - controls.gear_immediate = True - - self.last_neutral_btn = neutral_btn - self.last_forward_btn = forward_btn - self.last_reverse_btn = reverse_btn - self.last_park_btn = park_btn - self.last_shift_down_btn = shift_down_btn - self.last_shift_up_btn = shift_up_btn - - self.command_pub.publish(controls) - - def run(self): - rospy.spin() - -if __name__ == '__main__': - rospy.init_node('car_joy') - node = CarCommandTranslator() - node.run() diff --git a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp index 0a7f7316dc..5e9bf371bd 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -26,20 +26,19 @@ const std::unordered_map AirsimROSWrapper::image_type_int_to_s }; AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& host_ip) - : img_async_spinner_(1, &img_timer_cb_queue_) // a thread for image callbacks to be 'spun' by img_async_spinner_ - , lidar_async_spinner_(1, &lidar_timer_cb_queue_) // same as above, but for lidar - , is_used_lidar_timer_cb_queue_(false) - , is_used_img_timer_cb_queue_(false) - , nh_(nh) - , nh_private_(nh_private) - , host_ip_(host_ip) - , airsim_settings_parser_(host_ip) + : nh_(nh), nh_private_(nh_private), img_async_spinner_(1, &img_timer_cb_queue_), // a thread for image callbacks to be 'spun' by img_async_spinner_ + lidar_async_spinner_(1, &lidar_timer_cb_queue_) + , has_gimbal_cmd_(false) + , // same as above, but for lidar + host_ip_(host_ip) , airsim_client_images_(host_ip) , airsim_client_lidar_(host_ip) - , has_gimbal_cmd_(false) + , airsim_settings_parser_(host_ip) , tf_listener_(tf_buffer_) { ros_clock_.clock.fromSec(0); + is_used_lidar_timer_cb_queue_ = false; + is_used_img_timer_cb_queue_ = false; if (AirSimSettings::singleton().simmode_name != AirSimSettings::kSimModeTypeCar) { airsim_mode_ = AIRSIM_MODE::DRONE; @@ -159,34 +158,18 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() // bind to a single callback. todo optimal subs queue length // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument - drone->vel_cmd_body_frame_sub = nh_private_.subscribe( - curr_vehicle_name + "/vel_cmd_body_frame", - 1, - boost::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name)); - // TODO: ros::TransportHints().tcpNoDelay(); - - drone->vel_cmd_world_frame_sub = nh_private_.subscribe( - curr_vehicle_name + "/vel_cmd_world_frame", - 1, - boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name)); - - drone->takeoff_srvr = nh_private_.advertiseService( - curr_vehicle_name + "/takeoff", - boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name)); - - drone->land_srvr = nh_private_.advertiseService( - curr_vehicle_name + "/land", - boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name)); + drone->vel_cmd_body_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_body_frame", 1, boost::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name)); // todo ros::TransportHints().tcpNoDelay(); + drone->vel_cmd_world_frame_sub = nh_private_.subscribe(curr_vehicle_name + "/vel_cmd_world_frame", 1, boost::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name)); + drone->takeoff_srvr = nh_private_.advertiseService(curr_vehicle_name + "/takeoff", + boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name)); + drone->land_srvr = nh_private_.advertiseService(curr_vehicle_name + "/land", + boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name)); // vehicle_ros.reset_srvr = nh_private_.advertiseService(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); } else { auto car = static_cast(vehicle_ros.get()); - car->car_cmd_sub = nh_private_.subscribe( - curr_vehicle_name + "/car_cmd", - 1, - boost::bind(&AirsimROSWrapper::car_cmd_cb, this, _1, vehicle_ros->vehicle_name)); - + car->car_cmd_sub = nh_private_.subscribe(curr_vehicle_name + "/car_cmd", 1, boost::bind(&AirsimROSWrapper::car_cmd_cb, this, _1, vehicle_ros->vehicle_name)); car->car_state_pub = nh_private_.advertise(curr_vehicle_name + "/car_state", 10); } @@ -199,20 +182,18 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() append_static_camera_tf(vehicle_ros.get(), curr_camera_name, camera_setting); // camera_setting.gimbal std::vector current_image_request_vec; + current_image_request_vec.clear(); // iterate over capture_setting std::map capture_settings for (const auto& curr_capture_elem : camera_setting.capture_settings) { - const auto& capture_setting = curr_capture_elem.second; + auto& capture_setting = curr_capture_elem.second; // todo why does AirSimSettings::loadCaptureSettings calls AirSimSettings::initializeCaptureSettings() // which initializes default capture settings for _all_ NINE msr::airlib::ImageCaptureBase::ImageType if (!(std::isnan(capture_setting.fov_degrees))) { - const ImageType curr_image_type = msr::airlib::Utils::toEnum(capture_setting.image_type); + ImageType curr_image_type = msr::airlib::Utils::toEnum(capture_setting.image_type); // if scene / segmentation / surface normals / infrared, get uncompressed image with pixels_as_floats = false - if (curr_image_type == ImageType::Scene || - curr_image_type == ImageType::Segmentation || - curr_image_type == ImageType::SurfaceNormals || - curr_image_type == ImageType::Infrared) { + if (capture_setting.image_type == 0 || capture_setting.image_type == 5 || capture_setting.image_type == 6 || capture_setting.image_type == 7) { current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, false, false)); } // if {DepthPlanar, DepthPerspective,DepthVis, DisparityNormalized}, get float image @@ -220,53 +201,53 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, true)); } - const std::string cam_image_topic = curr_vehicle_name + "/" + curr_camera_name + "/" + - image_type_int_to_string_map_.at(capture_setting.image_type); - - image_pub_vec_.push_back(image_transporter.advertise(cam_image_topic, 1)); - cam_info_pub_vec_.push_back(nh_private_.advertise(cam_image_topic + "/camera_info", 10)); + image_pub_vec_.push_back(image_transporter.advertise(curr_vehicle_name + "/" + curr_camera_name + "/" + image_type_int_to_string_map_.at(capture_setting.image_type), 1)); + cam_info_pub_vec_.push_back(nh_private_.advertise(curr_vehicle_name + "/" + curr_camera_name + "/" + image_type_int_to_string_map_.at(capture_setting.image_type) + "/camera_info", 10)); camera_info_msg_vec_.push_back(generate_cam_info(curr_camera_name, camera_setting, capture_setting)); } } // push back pair (vector of image captures, current vehicle name) - airsim_img_request_vehicle_name_pair_vec_.push_back({ current_image_request_vec, curr_vehicle_name }); + airsim_img_request_vehicle_name_pair_vec_.push_back(std::make_pair(current_image_request_vec, curr_vehicle_name)); } // iterate over sensors std::vector sensors; - for (const auto& [sensor_name, sensor_setting] : vehicle_setting->sensors) { + for (auto& curr_sensor_map : vehicle_setting->sensors) { + auto& sensor_name = curr_sensor_map.first; + auto& sensor_setting = curr_sensor_map.second; + if (sensor_setting->enabled) { SensorPublisher sensor_publisher; - sensor_publisher.sensor_name = sensor_name; + sensor_publisher.sensor_name = sensor_setting->sensor_name; sensor_publisher.sensor_type = sensor_setting->sensor_type; switch (sensor_setting->sensor_type) { case SensorBase::SensorType::Barometer: { - ROS_INFO_STREAM(sensor_name << ": Barometer"); + std::cout << "Barometer" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/altimeter/" + sensor_name, 10); break; } case SensorBase::SensorType::Imu: { - ROS_INFO_STREAM(sensor_name << ": IMU"); + std::cout << "Imu" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/imu/" + sensor_name, 10); break; } case SensorBase::SensorType::Gps: { - ROS_INFO_STREAM(sensor_name << ": GPS"); + std::cout << "Gps" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/gps/" + sensor_name, 10); break; } case SensorBase::SensorType::Magnetometer: { - ROS_INFO_STREAM(sensor_name << ": Magnetometer"); + std::cout << "Magnetometer" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/magnetometer/" + sensor_name, 10); break; } case SensorBase::SensorType::Distance: { - ROS_INFO_STREAM(sensor_name << ": Distance sensor"); + std::cout << "Distance" << std::endl; sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/distance/" + sensor_name, 10); break; } case SensorBase::SensorType::Lidar: { - ROS_INFO_STREAM(sensor_name << ": Lidar"); + std::cout << "Lidar" << std::endl; auto lidar_setting = *static_cast(sensor_setting.get()); msr::airlib::LidarSimpleParams params; params.initializeFromSettings(lidar_setting); @@ -283,9 +264,9 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() } // we want fast access to the lidar sensors for callback handling, sort them out now - auto isLidar = [](const SensorPublisher& pub) { + auto isLidar = std::function([](const SensorPublisher& pub) { return pub.sensor_type == SensorBase::SensorType::Lidar; - }; + }); size_t cnt = std::count_if(sensors.begin(), sensors.end(), isLidar); lidar_cnt += cnt; vehicle_ros->lidar_pubs.resize(cnt); @@ -324,10 +305,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() double update_airsim_img_response_every_n_sec; nh_private_.getParam("update_airsim_img_response_every_n_sec", update_airsim_img_response_every_n_sec); - ros::TimerOptions timer_options(ros::Duration(update_airsim_img_response_every_n_sec), - boost::bind(&AirsimROSWrapper::img_response_timer_cb, this, _1), - &img_timer_cb_queue_); - + ros::TimerOptions timer_options(ros::Duration(update_airsim_img_response_every_n_sec), boost::bind(&AirsimROSWrapper::img_response_timer_cb, this, _1), &img_timer_cb_queue_); airsim_img_response_timer_ = nh_private_.createTimer(timer_options); is_used_img_timer_cb_queue_ = true; } @@ -337,11 +315,7 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() double update_lidar_every_n_sec; nh_private_.getParam("update_lidar_every_n_sec", update_lidar_every_n_sec); // nh_private_.setCallbackQueue(&lidar_timer_cb_queue_); - - ros::TimerOptions timer_options(ros::Duration(update_lidar_every_n_sec), - boost::bind(&AirsimROSWrapper::lidar_timer_cb, this, _1), - &lidar_timer_cb_queue_); - + ros::TimerOptions timer_options(ros::Duration(update_lidar_every_n_sec), boost::bind(&AirsimROSWrapper::lidar_timer_cb, this, _1), &lidar_timer_cb_queue_); airsim_lidar_update_timer_ = nh_private_.createTimer(timer_options); is_used_lidar_timer_cb_queue_ = true; } @@ -355,10 +329,10 @@ bool AirsimROSWrapper::takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - get_multirotor_client()->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else - get_multirotor_client()->takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = return true; @@ -370,11 +344,11 @@ bool AirsimROSWrapper::takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Reque if (request.waitOnLastTask) for (const auto& vehicle_name : request.vehicle_names) - get_multirotor_client()->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else for (const auto& vehicle_name : request.vehicle_names) - get_multirotor_client()->takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = return true; @@ -386,11 +360,11 @@ bool AirsimROSWrapper::takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& req if (request.waitOnLastTask) for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - get_multirotor_client()->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? // response.success = else for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - get_multirotor_client()->takeoffAsync(20, vehicle_name_ptr_pair.first); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first); // response.success = return true; @@ -401,9 +375,9 @@ bool AirsimROSWrapper::land_srv_cb(airsim_ros_pkgs::Land::Request& request, airs std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - get_multirotor_client()->landAsync(60, vehicle_name)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); else - get_multirotor_client()->landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); return true; //todo } @@ -414,10 +388,10 @@ bool AirsimROSWrapper::land_group_srv_cb(airsim_ros_pkgs::LandGroup::Request& re if (request.waitOnLastTask) for (const auto& vehicle_name : request.vehicle_names) - get_multirotor_client()->landAsync(60, vehicle_name)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); else for (const auto& vehicle_name : request.vehicle_names) - get_multirotor_client()->landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); return true; //todo } @@ -428,10 +402,10 @@ bool AirsimROSWrapper::land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, if (request.waitOnLastTask) for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - get_multirotor_client()->landAsync(60, vehicle_name_ptr_pair.first)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first)->waitOnLastTask(); else for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - get_multirotor_client()->landAsync(60, vehicle_name_ptr_pair.first); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first); return true; //todo } @@ -625,9 +599,7 @@ void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAn { try { tf2::Quaternion quat_control_cmd; - quat_control_cmd.setRPY(math_common::deg2rad(gimbal_angle_euler_cmd_msg.roll), - math_common::deg2rad(gimbal_angle_euler_cmd_msg.pitch), - math_common::deg2rad(gimbal_angle_euler_cmd_msg.yaw)); + quat_control_cmd.setRPY(math_common::deg2rad(gimbal_angle_euler_cmd_msg.roll), math_common::deg2rad(gimbal_angle_euler_cmd_msg.pitch), math_common::deg2rad(gimbal_angle_euler_cmd_msg.yaw)); quat_control_cmd.normalize(); gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); gimbal_cmd_.camera_name = gimbal_angle_euler_cmd_msg.camera_name; @@ -932,16 +904,6 @@ ros::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoin return cur_time; } -msr::airlib::MultirotorRpcLibClient* AirsimROSWrapper::get_multirotor_client() -{ - return static_cast(airsim_client_.get()); -} - -msr::airlib::CarRpcLibClient* AirsimROSWrapper::get_car_client() -{ - return static_cast(airsim_client_.get()); -} - void AirsimROSWrapper::drone_state_timer_cb(const ros::TimerEvent& event) { try { @@ -1005,7 +967,8 @@ ros::Time AirsimROSWrapper::update_state() if (airsim_mode_ == AIRSIM_MODE::DRONE) { auto drone = static_cast(vehicle_ros.get()); - drone->curr_drone_state = get_multirotor_client()->getMultirotorState(vehicle_ros->vehicle_name); + auto rpc = static_cast(airsim_client_.get()); + drone->curr_drone_state = rpc->getMultirotorState(vehicle_ros->vehicle_name); vehicle_time = airsim_timestamp_to_ros(drone->curr_drone_state.timestamp); if (!got_sim_time) { @@ -1020,7 +983,8 @@ ros::Time AirsimROSWrapper::update_state() } else { auto car = static_cast(vehicle_ros.get()); - car->curr_car_state = get_car_client()->getCarState(vehicle_ros->vehicle_name); + auto rpc = static_cast(airsim_client_.get()); + car->curr_car_state = rpc->getCarState(vehicle_ros->vehicle_name); vehicle_time = airsim_timestamp_to_ros(car->curr_car_state.timestamp); if (!got_sim_time) { @@ -1134,13 +1098,7 @@ void AirsimROSWrapper::update_commands() // send control commands from the last callback to airsim if (drone->has_vel_cmd) { std::lock_guard guard(drone_control_mutex_); - get_multirotor_client()->moveByVelocityAsync(drone->vel_cmd.x, - drone->vel_cmd.y, - drone->vel_cmd.z, - vel_cmd_duration_, - msr::airlib::DrivetrainType::MaxDegreeOfFreedom, - drone->vel_cmd.yaw_mode, - drone->vehicle_name); + static_cast(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd.x, drone->vel_cmd.y, drone->vel_cmd.z, vel_cmd_duration_, msr::airlib::DrivetrainType::MaxDegreeOfFreedom, drone->vel_cmd.yaw_mode, drone->vehicle_name); } drone->has_vel_cmd = false; } @@ -1149,7 +1107,7 @@ void AirsimROSWrapper::update_commands() auto car = static_cast(vehicle_ros.get()); if (car->has_car_cmd) { std::lock_guard guard(drone_control_mutex_); - get_car_client()->setCarControls(car->car_cmd, vehicle_ros->vehicle_name); + static_cast(airsim_client_.get())->setCarControls(car->car_cmd, vehicle_ros->vehicle_name); } car->has_car_cmd = false; } diff --git a/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp b/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp index 168eb02809..f03e49074d 100644 --- a/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp +++ b/ros/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp @@ -32,7 +32,7 @@ bool DynamicConstraints::load_from_rosparams(const ros::NodeHandle& nh) } PIDPositionController::PIDPositionController(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private) - : nh_(nh), nh_private_(nh_private), has_home_geo_(false), reached_goal_(false), has_goal_(false), has_odom_(false), got_goal_once_(false) + : nh_(nh), nh_private_(nh_private), has_odom_(false), has_goal_(false), reached_goal_(false), got_goal_once_(false), has_home_geo_(false), use_eth_lib_for_geodetic_conv_(true) { params_.load_from_rosparams(nh_private_); constraints_.load_from_rosparams(nh_); @@ -161,7 +161,7 @@ void PIDPositionController::home_geopoint_cb(const airsim_ros_pkgs::GPSYaw& gps_ gps_home_msg_ = gps_msg; has_home_geo_ = true; ROS_INFO_STREAM("[PIDPositionController] GPS reference initializing " << gps_msg.latitude << ", " << gps_msg.longitude << ", " << gps_msg.altitude); - geodetic_converter_.setHome(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude); + geodetic_converter_.initialiseReference(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude); } // todo do relative altitude, or add an option for the same? @@ -177,16 +177,21 @@ bool PIDPositionController::gps_goal_srv_cb(airsim_ros_pkgs::SetGPSPosition::Req if (!has_goal_) { msr::airlib::GeoPoint goal_gps_point(request.latitude, request.longitude, request.altitude); msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); - if constexpr (use_eth_lib_for_geodetic_conv_) { + bool use_eth_lib = true; + if (use_eth_lib_for_geodetic_conv_) { + double initial_latitude, initial_longitude, initial_altitude; + geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); double n, e, d; geodetic_converter_.geodetic2Ned(request.latitude, request.longitude, request.altitude, &n, &e, &d); + // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); target_position_.x = n; target_position_.y = e; target_position_.z = d; } else // use airlib::GeodeticToNedFast { - ROS_INFO_STREAM("[PIDPositionController] home geopoint: " << gps_home); + ROS_INFO_STREAM("[PIDPositionController] home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" + << "todo"); msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); target_position_.x = ned_goal[0]; target_position_.y = ned_goal[1]; @@ -222,16 +227,21 @@ bool PIDPositionController::gps_goal_srv_override_cb(airsim_ros_pkgs::SetGPSPosi msr::airlib::GeoPoint goal_gps_point(request.latitude, request.longitude, request.altitude); msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); - if constexpr (use_eth_lib_for_geodetic_conv_) { + bool use_eth_lib = true; + if (use_eth_lib_for_geodetic_conv_) { + double initial_latitude, initial_longitude, initial_altitude; + geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); double n, e, d; geodetic_converter_.geodetic2Ned(request.latitude, request.longitude, request.altitude, &n, &e, &d); + // ROS_INFO_STREAM("[PIDPositionController] geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); target_position_.x = n; target_position_.y = e; target_position_.z = d; } else // use airlib::GeodeticToNedFast { - ROS_INFO_STREAM("[PIDPositionController] home geopoint: " << gps_home); + ROS_INFO_STREAM("[PIDPositionController] home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" + << "todo"); msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); target_position_.x = ned_goal[0]; target_position_.y = ned_goal[1]; @@ -329,4 +339,4 @@ void PIDPositionController::enforce_dynamic_constraints() void PIDPositionController::publish_control_cmd() { airsim_vel_cmd_world_frame_pub_.publish(vel_cmd_); -} +} \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/CMakeLists.txt b/ros2/src/airsim_interfaces/CMakeLists.txt deleted file mode 100644 index 7a5edbb03b..0000000000 --- a/ros2/src/airsim_interfaces/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -cmake_minimum_required(VERSION 3.5) - -project(airsim_interfaces) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(geographic_msgs REQUIRED) - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/GimbalAngleEulerCmd.msg" - "msg/GimbalAngleQuatCmd.msg" - "msg/GPSYaw.msg" - "msg/VelCmd.msg" - "msg/VelCmdGroup.msg" - "msg/CarControls.msg" - "msg/CarState.msg" - "msg/Altimeter.msg" - "msg/Environment.msg" - "srv/SetGPSPosition.srv" - "srv/Takeoff.srv" - "srv/TakeoffGroup.srv" - "srv/Land.srv" - "srv/LandGroup.srv" - "srv/Reset.srv" - "srv/SetLocalPosition.srv" DEPENDENCIES std_msgs geometry_msgs geographic_msgs -) - -ament_export_dependencies(rosidl_default_runtime) -ament_package() diff --git a/ros2/src/airsim_interfaces/msg/Altimeter.msg b/ros2/src/airsim_interfaces/msg/Altimeter.msg deleted file mode 100755 index 34e3dc1d70..0000000000 --- a/ros2/src/airsim_interfaces/msg/Altimeter.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header -float32 altitude -float32 pressure -float32 qnh \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/CarControls.msg b/ros2/src/airsim_interfaces/msg/CarControls.msg deleted file mode 100755 index a6a2fd23c6..0000000000 --- a/ros2/src/airsim_interfaces/msg/CarControls.msg +++ /dev/null @@ -1,8 +0,0 @@ -std_msgs/Header header -float32 throttle -float32 brake -float32 steering -bool handbrake -bool manual -int8 manual_gear -bool gear_immediate \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/CarState.msg b/ros2/src/airsim_interfaces/msg/CarState.msg deleted file mode 100755 index e5bde5a59d..0000000000 --- a/ros2/src/airsim_interfaces/msg/CarState.msg +++ /dev/null @@ -1,8 +0,0 @@ -std_msgs/Header header -geometry_msgs/PoseWithCovariance pose -geometry_msgs/TwistWithCovariance twist -float32 speed -int8 gear -float32 rpm -float32 maxrpm -bool handbrake \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/Environment.msg b/ros2/src/airsim_interfaces/msg/Environment.msg deleted file mode 100755 index fdeed971cc..0000000000 --- a/ros2/src/airsim_interfaces/msg/Environment.msg +++ /dev/null @@ -1,8 +0,0 @@ -std_msgs/Header header -geometry_msgs/Vector3 position -geographic_msgs/GeoPoint geo_point -geometry_msgs/Vector3 gravity -float32 air_pressure -float32 temperature -float32 air_density - diff --git a/ros2/src/airsim_interfaces/msg/GPSYaw.msg b/ros2/src/airsim_interfaces/msg/GPSYaw.msg deleted file mode 100755 index 1e03ea33a4..0000000000 --- a/ros2/src/airsim_interfaces/msg/GPSYaw.msg +++ /dev/null @@ -1,4 +0,0 @@ -float64 latitude -float64 longitude -float64 altitude -float64 yaw \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/GimbalAngleEulerCmd.msg b/ros2/src/airsim_interfaces/msg/GimbalAngleEulerCmd.msg deleted file mode 100755 index cf3cdf4bc8..0000000000 --- a/ros2/src/airsim_interfaces/msg/GimbalAngleEulerCmd.msg +++ /dev/null @@ -1,6 +0,0 @@ -std_msgs/Header header -string camera_name -string vehicle_name -float64 roll -float64 pitch -float64 yaw \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/GimbalAngleQuatCmd.msg b/ros2/src/airsim_interfaces/msg/GimbalAngleQuatCmd.msg deleted file mode 100755 index 819dd53c52..0000000000 --- a/ros2/src/airsim_interfaces/msg/GimbalAngleQuatCmd.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header -string camera_name -string vehicle_name -geometry_msgs/Quaternion orientation \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/VelCmd.msg b/ros2/src/airsim_interfaces/msg/VelCmd.msg deleted file mode 100755 index 060f1e2873..0000000000 --- a/ros2/src/airsim_interfaces/msg/VelCmd.msg +++ /dev/null @@ -1 +0,0 @@ -geometry_msgs/Twist twist \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg b/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg deleted file mode 100755 index 1be795d0a7..0000000000 --- a/ros2/src/airsim_interfaces/msg/VelCmdGroup.msg +++ /dev/null @@ -1,2 +0,0 @@ -airsim_interfaces/VelCmd vel_cmd -string[] vehicle_names \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/package.xml b/ros2/src/airsim_interfaces/package.xml deleted file mode 100644 index 9b19439500..0000000000 --- a/ros2/src/airsim_interfaces/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - airsim_interfaces - 0.1.0 - Contains message and service definitions used by the examples. - AAA - Apache License 2.0 - - AAA - - ament_cmake - - rosidl_default_generators - - std_msgs - geometry_msgs - geographic_msgs - - rosidl_default_runtime - - rosidl_interface_packages - - - ament_cmake - - diff --git a/ros2/src/airsim_interfaces/srv/Land.srv b/ros2/src/airsim_interfaces/srv/Land.srv deleted file mode 100755 index 2100b641c0..0000000000 --- a/ros2/src/airsim_interfaces/srv/Land.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool wait_on_last_task ---- -bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/LandGroup.srv b/ros2/src/airsim_interfaces/srv/LandGroup.srv deleted file mode 100755 index 1c900c14b8..0000000000 --- a/ros2/src/airsim_interfaces/srv/LandGroup.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] vehicle_names -bool wait_on_last_task ---- -bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/Reset.srv b/ros2/src/airsim_interfaces/srv/Reset.srv deleted file mode 100755 index 3e754478e9..0000000000 --- a/ros2/src/airsim_interfaces/srv/Reset.srv +++ /dev/null @@ -1,4 +0,0 @@ -# string vehicle_name -bool wait_on_last_task ---- -bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/SetGPSPosition.srv b/ros2/src/airsim_interfaces/srv/SetGPSPosition.srv deleted file mode 100755 index 76cc2c081b..0000000000 --- a/ros2/src/airsim_interfaces/srv/SetGPSPosition.srv +++ /dev/null @@ -1,8 +0,0 @@ -float64 latitude -float64 longitude -float64 altitude -float64 yaw -string vehicle_name ---- -#Response : return success=true, (if async=false && if setpoint reached before timeout = 30sec) || (if async=true && command sent to autopilot) -bool success \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/SetLocalPosition.srv b/ros2/src/airsim_interfaces/srv/SetLocalPosition.srv deleted file mode 100755 index f6293fc6e6..0000000000 --- a/ros2/src/airsim_interfaces/srv/SetLocalPosition.srv +++ /dev/null @@ -1,11 +0,0 @@ -#Request : expects position setpoint via x, y, z -#Request : expects yaw setpoint via yaw (send yaw_valid=true) -float64 x -float64 y -float64 z -float64 yaw -string vehicle_name ---- -#Response : success=true - (if async=false && if setpoint reached before timeout = 30sec) || (if async=true) -bool success -string message \ No newline at end of file diff --git a/ros2/src/airsim_interfaces/srv/Takeoff.srv b/ros2/src/airsim_interfaces/srv/Takeoff.srv deleted file mode 100755 index 55472170ea..0000000000 --- a/ros2/src/airsim_interfaces/srv/Takeoff.srv +++ /dev/null @@ -1,3 +0,0 @@ -bool wait_on_last_task ---- -bool success diff --git a/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv b/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv deleted file mode 100755 index 1c900c14b8..0000000000 --- a/ros2/src/airsim_interfaces/srv/TakeoffGroup.srv +++ /dev/null @@ -1,4 +0,0 @@ -string[] vehicle_names -bool wait_on_last_task ---- -bool success \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/CMakeLists.txt b/ros2/src/airsim_ros_pkgs/CMakeLists.txt deleted file mode 100644 index 6d961a430f..0000000000 --- a/ros2/src/airsim_ros_pkgs/CMakeLists.txt +++ /dev/null @@ -1,136 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(airsim_ros_pkgs) - -find_package(rclcpp REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(geographic_msgs REQUIRED) -find_package(std_srvs REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(tf2_sensor_msgs REQUIRED) -find_package(rclpy REQUIRED) -find_package(tf2 REQUIRED) -find_package(image_transport REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(mavros_msgs REQUIRED) -find_package(rosidl_default_generators REQUIRED) -find_package(ament_cmake REQUIRED) -find_package(std_msgs REQUIRED) -find_package(rosidl_typesupport_cpp REQUIRED) -find_package(rosidl_default_runtime REQUIRED) -find_package(airsim_interfaces REQUIRED) -find_package(OpenCV REQUIRED) - -set(AIRSIM_ROOT ${CMAKE_CURRENT_SOURCE_DIR}/../../../) - -add_subdirectory("${AIRSIM_ROOT}/cmake/rpclib_wrapper" rpclib_wrapper) -add_subdirectory("${AIRSIM_ROOT}/cmake/AirLib" AirLib) -add_subdirectory("${AIRSIM_ROOT}/cmake/MavLinkCom" MavLinkCom) - -set(CMAKE_CXX_STANDARD 14) -set(CMAKE_CXX_FLAGS "-O3 -Wall -Wextra -Wnoexcept -Wstrict-null-sentinel") -set(CXX_EXP_LIB "-nostdinc++ -I/usr/include/c++/8 -I/usr/include/x86_64-linux-gnu/c++/8 -nodefaultlibs --l/usr/lib/x86_64-linux-gnu/libc++.so -l/usr/lib/x86_64-linux-gnu/libc++abi.so --lm -lc -lgcc_s -lgcc --lstdc++fs -fmax-errors=10") -set(RPC_LIB_INCLUDES " ${AIRSIM_ROOT}/external/rpclib/rpclib-2.3.0/include") -set(RPC_LIB rpc) -message(STATUS "found RPC_LIB_INCLUDES=${RPC_LIB_INCLUDES}") - -set(INCLUDE_DIRS include - ${AIRSIM_ROOT}/AirLib/deps/eigen3 - ${AIRSIM_ROOT}/AirLib/include - ${RPC_LIB_INCLUDES} - ${AIRSIM_ROOT}/MavLinkCom/include - ${AIRSIM_ROOT}/MavLinkCom/common_utils - ${OpenCV_INCLUDE_DIRS} - ) -include_directories(${INCLUDE_DIRS}) - -add_library(airsim_settings_parser src/airsim_settings_parser.cpp) -target_link_libraries(airsim_settings_parser AirLib) - -add_library(pd_position_controller_simple src/pd_position_controller_simple.cpp) -target_link_libraries(pd_position_controller_simple AirLib) -ament_target_dependencies(pd_position_controller_simple - rclcpp - nav_msgs - geometry_msgs - airsim_interfaces -) - -add_library(airsim_ros src/airsim_ros_wrapper.cpp) -target_link_libraries(airsim_ros ${OpenCV_LIBS} yaml-cpp AirLib airsim_settings_parser) -ament_target_dependencies(airsim_ros - rclcpp - sensor_msgs - geometry_msgs - nav_msgs - image_transport - tf2_ros - cv_bridge - airsim_interfaces -) - -add_executable(airsim_node src/airsim_node.cpp) -target_link_libraries(airsim_node airsim_ros AirLib) -ament_target_dependencies(airsim_node - rclcpp -) - -add_executable(pd_position_controller_simple_node - src/pd_position_controller_simple_node.cpp) -target_link_libraries(pd_position_controller_simple_node - pd_position_controller_simple airsim_ros AirLib) -ament_target_dependencies(pd_position_controller_simple_node - rclcpp -) - -# rosidl_target_interfaces(airsim_node -# ${PROJECT_NAME} "rosidl_typesupport_cpp") - -# rosidl_target_interfaces(pd_position_controller_simple_node -# ${PROJECT_NAME} "rosidl_typesupport_cpp") - -install(TARGETS airsim_node pd_position_controller_simple_node - DESTINATION lib/${PROJECT_NAME}) - -install(TARGETS airsim_ros pd_position_controller_simple - ARCHIVE - DESTINATION lib - LIBRARY - DESTINATION lib) - -install(FILES README.md DESTINATION share/${PROJECT_NAME}) - -install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) - - - -ament_export_dependencies(rclcpp) -ament_export_dependencies(nav_msgs) -ament_export_dependencies(geographic_msgs) -ament_export_dependencies(std_srvs) -ament_export_dependencies(tf2_ros) -ament_export_dependencies(tf2_sensor_msgs) -ament_export_dependencies(rclpy) -ament_export_dependencies(tf2) -ament_export_dependencies(image_transport) -ament_export_dependencies(geometry_msgs) -ament_export_dependencies(cv_bridge) -ament_export_dependencies(tf2_geometry_msgs) -ament_export_dependencies(sensor_msgs) -ament_export_dependencies(mavros_msgs) -ament_export_dependencies(rosidl_default_generators) -ament_export_dependencies(ament_cmake) -ament_export_dependencies(std_msgs) -ament_export_dependencies(rosidl_default_runtime) -ament_export_dependencies(airsim_interfaces) - -ament_export_include_directories(${INCLUDE_DIRS}) -ament_export_libraries(airsim_settings_parser pd_position_controller_simple - airsim_ros) - -ament_package() diff --git a/ros2/src/airsim_ros_pkgs/README.md b/ros2/src/airsim_ros_pkgs/README.md deleted file mode 100755 index 3f32237f5a..0000000000 --- a/ros2/src/airsim_ros_pkgs/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# airsim_ros_pkgs - -This page has moved [here](https://github.com/microsoft/AirSim/blob/master/docs/airsim_ros_pkgs.md). \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h b/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h deleted file mode 100644 index cf94c080d9..0000000000 --- a/ros2/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ /dev/null @@ -1,376 +0,0 @@ -#include "common/common_utils/StrictMode.hpp" -STRICT_MODE_OFF //todo what does this do? -#ifndef RPCLIB_MSGPACK -#define RPCLIB_MSGPACK clmdep_msgpack -#endif // !RPCLIB_MSGPACK -#include "rpc/rpc_error.h" - STRICT_MODE_ON - -#include "airsim_settings_parser.h" -#include "common/AirSimSettings.hpp" -#include "common/common_utils/FileSystem.hpp" -#include "sensors/lidar/LidarSimpleParams.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensors/imu/ImuBase.hpp" -#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" -#include "vehicles/car/api/CarRpcLibClient.hpp" -#include "yaml-cpp/yaml.h" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include //hector_uav_msgs defunct? -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - // todo move airlib typedefs to separate header file? - typedef msr::airlib::ImageCaptureBase::ImageRequest ImageRequest; -typedef msr::airlib::ImageCaptureBase::ImageResponse ImageResponse; -typedef msr::airlib::ImageCaptureBase::ImageType ImageType; -typedef msr::airlib::AirSimSettings::CaptureSetting CaptureSetting; -typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting; -typedef msr::airlib::AirSimSettings::CameraSetting CameraSetting; -typedef msr::airlib::AirSimSettings::LidarSetting LidarSetting; - -struct SimpleMatrix -{ - int rows; - int cols; - double* data; - - SimpleMatrix(int rows, int cols, double* data) - : rows(rows), cols(cols), data(data) - { - } -}; - -struct VelCmd -{ - double x; - double y; - double z; - msr::airlib::DrivetrainType drivetrain; - msr::airlib::YawMode yaw_mode; - std::string vehicle_name; -}; - -struct GimbalCmd -{ - std::string vehicle_name; - std::string camera_name; - msr::airlib::Quaternionr target_quat; -}; - -template -struct SensorPublisher -{ - SensorBase::SensorType sensor_type; - std::string sensor_name; - typename rclcpp::Publisher::SharedPtr publisher; -}; - -class AirsimROSWrapper -{ -public: - enum class AIRSIM_MODE : unsigned - { - DRONE, - CAR - }; - - AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip); - ~AirsimROSWrapper(){}; - - void initialize_airsim(); - void initialize_ros(); - - bool is_used_lidar_timer_cb_queue_; - bool is_used_img_timer_cb_queue_; - -private: - // utility struct for a SINGLE robot - class VehicleROS - { - public: - virtual ~VehicleROS() {} - std::string vehicle_name_; - - /// All things ROS - rclcpp::Publisher::SharedPtr odom_local_pub_; - rclcpp::Publisher::SharedPtr global_gps_pub_; - rclcpp::Publisher::SharedPtr env_pub_; - airsim_interfaces::msg::Environment env_msg_; - - std::vector> barometer_pubs_; - std::vector> imu_pubs_; - std::vector> gps_pubs_; - std::vector> magnetometer_pubs_; - std::vector> distance_pubs_; - std::vector> lidar_pubs_; - - // handle lidar seperately for max performance as data is collected on its own thread/callback - - nav_msgs::msg::Odometry curr_odom_; - sensor_msgs::msg::NavSatFix gps_sensor_msg_; - - std::vector static_tf_msg_vec_; - - rclcpp::Time stamp_; - - std::string odom_frame_id_; - }; - - class CarROS : public VehicleROS - { - public: - msr::airlib::CarApiBase::CarState curr_car_state_; - - rclcpp::Subscription::SharedPtr car_cmd_sub_; - rclcpp::Publisher::SharedPtr car_state_pub_; - airsim_interfaces::msg::CarState car_state_msg_; - - bool has_car_cmd_; - msr::airlib::CarApiBase::CarControls car_cmd_; - }; - - class MultiRotorROS : public VehicleROS - { - public: - /// State - msr::airlib::MultirotorState curr_drone_state_; - - rclcpp::Subscription::SharedPtr vel_cmd_body_frame_sub_; - rclcpp::Subscription::SharedPtr vel_cmd_world_frame_sub_; - - rclcpp::Service::SharedPtr takeoff_srvr_; - rclcpp::Service::SharedPtr land_srvr_; - - bool has_vel_cmd_; - VelCmd vel_cmd_; - }; - - /// ROS timer callbacks - void img_response_timer_cb(); // update images from airsim_client_ every nth sec - void drone_state_timer_cb(); // update drone state from airsim_client_ every nth sec - void lidar_timer_cb(); - - /// ROS subscriber callbacks - void vel_cmd_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name); - void vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name); - - void vel_cmd_group_body_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg); - void vel_cmd_group_world_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg); - - void vel_cmd_all_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg); - void vel_cmd_all_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg); - - // void vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd& msg, const std::string& vehicle_name); - void gimbal_angle_quat_cmd_cb(const airsim_interfaces::msg::GimbalAngleQuatCmd::SharedPtr gimbal_angle_quat_cmd_msg); - void gimbal_angle_euler_cmd_cb(const airsim_interfaces::msg::GimbalAngleEulerCmd::SharedPtr gimbal_angle_euler_cmd_msg); - - // commands - void car_cmd_cb(const airsim_interfaces::msg::CarControls::SharedPtr msg, const std::string& vehicle_name); - void update_commands(); - - // state, returns the simulation timestamp best guess based on drone state timestamp, airsim needs to return timestap for environment - rclcpp::Time update_state(); - void update_and_publish_static_transforms(VehicleROS* vehicle_ros); - void publish_vehicle_state(); - - /// ROS service callbacks - bool takeoff_srv_cb(const std::shared_ptr request, const std::shared_ptr response, const std::string& vehicle_name); - bool takeoff_group_srv_cb(const std::shared_ptr request, const std::shared_ptr response); - bool takeoff_all_srv_cb(const std::shared_ptr request, const std::shared_ptr response); - bool land_srv_cb(const std::shared_ptr request, const std::shared_ptr response, const std::string& vehicle_name); - bool land_group_srv_cb(const std::shared_ptr request, const std::shared_ptr response); - bool land_all_srv_cb(const std::shared_ptr request, const std::shared_ptr response); - bool reset_srv_cb(const std::shared_ptr request, const std::shared_ptr response); - - /// ROS tf broadcasters - void publish_camera_tf(const ImageResponse& img_response, const rclcpp::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id); - void publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg); - - /// camera helper methods - sensor_msgs::msg::CameraInfo generate_cam_info(const std::string& camera_name, const CameraSetting& camera_setting, const CaptureSetting& capture_setting) const; - cv::Mat manual_decode_depth(const ImageResponse& img_response) const; - - std::shared_ptr get_img_msg_from_response(const ImageResponse& img_response, const rclcpp::Time curr_ros_time, const std::string frame_id); - std::shared_ptr get_depth_img_msg_from_response(const ImageResponse& img_response, const rclcpp::Time curr_ros_time, const std::string frame_id); - - void process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name); - - // methods which parse setting json ang generate ros pubsubsrv - void create_ros_pubs_from_settings_json(); - void convert_tf_msg_to_enu(geometry_msgs::msg::TransformStamped& tf_msg); - void append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting); - void append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting); - void append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting); - void set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const; - void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const; - void set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, LidarSetting& lidar_setting) const; - - /// utils. todo parse into an Airlib<->ROS conversion class - tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const; - msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const; - msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const; - nav_msgs::msg::Odometry get_odom_msg_from_kinematic_state(const msr::airlib::Kinematics::State& kinematics_estimated) const; - nav_msgs::msg::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const; - nav_msgs::msg::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; - airsim_interfaces::msg::CarState get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; - msr::airlib::Pose get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const; - airsim_interfaces::msg::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; - sensor_msgs::msg::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; - sensor_msgs::msg::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const; - airsim_interfaces::msg::Altimeter get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const; - sensor_msgs::msg::Range get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const; - sensor_msgs::msg::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const; - sensor_msgs::msg::NavSatFix get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const; - sensor_msgs::msg::MagneticField get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const; - airsim_interfaces::msg::Environment get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const; - msr::airlib::GeoPoint get_origin_geo_point() const; - VelCmd get_airlib_world_vel_cmd(const airsim_interfaces::msg::VelCmd& msg) const; - VelCmd get_airlib_body_vel_cmd(const airsim_interfaces::msg::VelCmd& msg, const msr::airlib::Quaternionr& airlib_quat) const; - geometry_msgs::msg::Transform get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::AirSimSettings::Rotation& rotation); - geometry_msgs::msg::Transform get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::Quaternionr& quaternion); - - // not used anymore, but can be useful in future with an unreal camera calibration environment - void read_params_from_yaml_and_fill_cam_info_msg(const std::string& file_name, sensor_msgs::msg::CameraInfo& cam_info) const; - void convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const; // todo ugly - - // simulation time utility - rclcpp::Time airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const; - rclcpp::Time chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const; - - template - const SensorPublisher create_sensor_publisher(const string& sensor_type_name, const string& sensor_name, SensorBase::SensorType sensor_type, const string& topic_name, int QoS); - -private: - // subscriber / services for ALL robots - rclcpp::Subscription::SharedPtr vel_cmd_all_body_frame_sub_; - rclcpp::Subscription::SharedPtr vel_cmd_all_world_frame_sub_; - rclcpp::Service::SharedPtr takeoff_all_srvr_; - rclcpp::Service::SharedPtr land_all_srvr_; - - // todo - subscriber / services for a GROUP of robots, which is defined by a list of `vehicle_name`s passed in the ros msg / srv request - rclcpp::Subscription::SharedPtr vel_cmd_group_body_frame_sub_; - rclcpp::Subscription::SharedPtr vel_cmd_group_world_frame_sub_; - rclcpp::Service::SharedPtr takeoff_group_srvr_; - rclcpp::Service::SharedPtr land_group_srvr_; - - AIRSIM_MODE airsim_mode_ = AIRSIM_MODE::DRONE; - - rclcpp::Service::SharedPtr reset_srvr_; - rclcpp::Publisher::SharedPtr origin_geo_point_pub_; // home geo coord of drones - msr::airlib::GeoPoint origin_geo_point_; // gps coord of unreal origin - airsim_interfaces::msg::GPSYaw origin_geo_point_msg_; // todo duplicate - - AirSimSettingsParser airsim_settings_parser_; - std::unordered_map> vehicle_name_ptr_map_; - static const std::unordered_map image_type_int_to_string_map_; - - bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB - - std::string host_ip_; - std::unique_ptr airsim_client_; - // seperate busy connections to airsim, update in their own thread - msr::airlib::RpcLibClientBase airsim_client_images_; - msr::airlib::RpcLibClientBase airsim_client_lidar_; - - std::shared_ptr nh_; - std::shared_ptr nh_img_; - std::shared_ptr nh_lidar_; - - // todo not sure if async spinners shuold be inside this class, or should be instantiated in airsim_node.cpp, and cb queues should be public - // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS? - - std::mutex control_mutex_; - - // gimbal control - bool has_gimbal_cmd_; - GimbalCmd gimbal_cmd_; - - /// ROS tf - const std::string AIRSIM_FRAME_ID = "world_ned"; - std::string world_frame_id_ = AIRSIM_FRAME_ID; - const std::string AIRSIM_ODOM_FRAME_ID = "odom_local_ned"; - const std::string ENU_ODOM_FRAME_ID = "odom_local_enu"; - std::string odom_frame_id_ = AIRSIM_ODOM_FRAME_ID; - std::shared_ptr tf_broadcaster_; - std::shared_ptr static_tf_pub_; - - bool isENU_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - - /// ROS params - double vel_cmd_duration_; - - /// ROS Timers. - rclcpp::TimerBase::SharedPtr airsim_img_response_timer_; - rclcpp::TimerBase::SharedPtr airsim_control_update_timer_; - rclcpp::TimerBase::SharedPtr airsim_lidar_update_timer_; - - 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_; - std::vector::SharedPtr> cam_info_pub_vec_; - - std::vector camera_info_msg_vec_; - - /// ROS other publishers - rclcpp::Publisher::SharedPtr clock_pub_; - rosgraph_msgs::msg::Clock ros_clock_; - bool publish_clock_; - - rclcpp::Subscription::SharedPtr gimbal_angle_quat_cmd_sub_; - rclcpp::Subscription::SharedPtr gimbal_angle_euler_cmd_sub_; - - static constexpr char CAM_YML_NAME[] = "camera_name"; - static constexpr char WIDTH_YML_NAME[] = "image_width"; - static constexpr char HEIGHT_YML_NAME[] = "image_height"; - static constexpr char K_YML_NAME[] = "camera_matrix"; - static constexpr char D_YML_NAME[] = "distortion_coefficients"; - static constexpr char R_YML_NAME[] = "rectification_matrix"; - static constexpr char P_YML_NAME[] = "projection_matrix"; - static constexpr char DMODEL_YML_NAME[] = "distortion_model"; -}; \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h b/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h deleted file mode 100755 index 2124ddff5e..0000000000 --- a/ros2/src/airsim_ros_pkgs/include/airsim_settings_parser.h +++ /dev/null @@ -1,35 +0,0 @@ -#include "common/common_utils/StrictMode.hpp" -STRICT_MODE_OFF -#ifndef RPCLIB_MSGPACK -#define RPCLIB_MSGPACK clmdep_msgpack -#endif // !RPCLIB_MSGPACK -#include "rpc/rpc_error.h" -STRICT_MODE_ON - -#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" -#include "common/common_utils/FileSystem.hpp" -#include -#include -#include "common/AirSimSettings.hpp" - -// a minimal airsim settings parser, adapted from Unreal/Plugins/AirSim/SimHUD/SimHUD.h -class AirSimSettingsParser -{ -public: - typedef msr::airlib::AirSimSettings AirSimSettings; - -public: - AirSimSettingsParser(const std::string& host_ip); - ~AirSimSettingsParser() = default; - - bool success(); - -private: - std::string getSimMode(); - bool getSettingsText(std::string& settings_text) const; - bool initializeSettings(); - - bool success_; - std::string settings_text_; - std::string host_ip_; -}; \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/math_common.h b/ros2/src/airsim_ros_pkgs/include/math_common.h deleted file mode 100755 index 375a035aa7..0000000000 --- a/ros2/src/airsim_ros_pkgs/include/math_common.h +++ /dev/null @@ -1,45 +0,0 @@ -namespace math_common -{ -template -inline T rad2deg(const T radians) -{ - return (radians / M_PI) * 180.0; -} - -template -inline T deg2rad(const T degrees) -{ - return (degrees / 180.0) * M_PI; -} - -template -inline T wrap_to_pi(T radians) -{ - int m = (int)(radians / (2 * M_PI)); - radians = radians - m * 2 * M_PI; - if (radians > M_PI) - radians -= 2.0 * M_PI; - else if (radians < -M_PI) - radians += 2.0 * M_PI; - return radians; -} - -template -inline void wrap_to_pi_inplace(T& a) -{ - a = wrap_to_pi(a); -} - -template -inline T angular_dist(T from, T to) -{ - wrap_to_pi_inplace(from); - wrap_to_pi_inplace(to); - T d = to - from; - if (d > M_PI) - d -= 2. * M_PI; - else if (d < -M_PI) - d += 2. * M_PI; - return d; -} -} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h b/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h deleted file mode 100644 index b2424b27f3..0000000000 --- a/ros2/src/airsim_ros_pkgs/include/pd_position_controller_simple.h +++ /dev/null @@ -1,139 +0,0 @@ -#ifndef _PID_POSITION_CONTROLLER_SIMPLE_H_ -#define _PID_POSITION_CONTROLLER_SIMPLE_H_ - -#include "common/common_utils/StrictMode.hpp" -STRICT_MODE_OFF //todo what does this do? -#ifndef RPCLIB_MSGPACK -#define RPCLIB_MSGPACK clmdep_msgpack -#endif // !RPCLIB_MSGPACK -#include "rpc/rpc_error.h" - STRICT_MODE_ON - -#include "common/common_utils/FileSystem.hpp" -#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - // todo nicer api - class PIDParams -{ -public: - double kp_x; - double kp_y; - double kp_z; - double kp_yaw; - double kd_x; - double kd_y; - double kd_z; - double kd_yaw; - - double reached_thresh_xyz; - double reached_yaw_degrees; - - PIDParams() - : kp_x(0.5), kp_y(0.5), kp_z(0.5), kp_yaw(0.5), kd_x(0.1), kd_y(0.1), kd_z(0.1), kd_yaw(0.1), reached_thresh_xyz(0.5), reached_yaw_degrees(5.0) - { - } - - bool load_from_rosparams(const std::shared_ptr nh); -}; - -// todo should be a common representation -struct XYZYaw -{ - double x; - double y; - double z; - double yaw; -}; - -// todo should be a common representation -class DynamicConstraints -{ -public: - double max_vel_horz_abs; // meters/sec - double max_vel_vert_abs; - double max_yaw_rate_degree; - - DynamicConstraints() - : max_vel_horz_abs(1.0), max_vel_vert_abs(0.5), max_yaw_rate_degree(10.0) - { - } - - bool load_from_rosparams(const std::shared_ptr nh); -}; - -class PIDPositionController -{ -public: - PIDPositionController(const std::shared_ptr nh); - - // ROS service callbacks - bool local_position_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response); - bool local_position_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response); - bool gps_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response); - bool gps_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response); - - // ROS subscriber callbacks - void airsim_odom_cb(const nav_msgs::msg::Odometry::SharedPtr odom_msg); - void home_geopoint_cb(const airsim_interfaces::msg::GPSYaw::SharedPtr gps_msg); - - void update_control_cmd_timer_cb(); - - void reset_errors(); - - void initialize_ros(); - void compute_control_cmd(); - void enforce_dynamic_constraints(); - void publish_control_cmd(); - void check_reached_goal(); - -private: - geodetic_converter::GeodeticConverter geodetic_converter_; - const bool use_eth_lib_for_geodetic_conv_; - - const std::shared_ptr nh_; - - DynamicConstraints constraints_; - PIDParams params_; - XYZYaw target_position_; - XYZYaw curr_position_; - XYZYaw prev_error_; - XYZYaw curr_error_; - - bool has_home_geo_; - airsim_interfaces::msg::GPSYaw gps_home_msg_; - - nav_msgs::msg::Odometry curr_odom_; - airsim_interfaces::msg::VelCmd vel_cmd_; - bool reached_goal_; - bool has_goal_; - bool has_odom_; - bool got_goal_once_; - // todo check for odom msg being older than n sec - - rclcpp::Publisher::SharedPtr airsim_vel_cmd_world_frame_pub_; - rclcpp::Subscription::SharedPtr airsim_odom_sub_; - rclcpp::Subscription::SharedPtr home_geopoint_sub_; - - rclcpp::Service::SharedPtr local_position_goal_srvr_; - rclcpp::Service::SharedPtr local_position_goal_override_srvr_; - rclcpp::Service::SharedPtr gps_goal_srvr_; - rclcpp::Service::SharedPtr gps_goal_override_srvr_; - - rclcpp::TimerBase::SharedPtr update_control_cmd_timer_; -}; - -#endif /* _PID_POSITION_CONTROLLER_SIMPLE_ */ \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/include/utils.h b/ros2/src/airsim_ros_pkgs/include/utils.h deleted file mode 100755 index a62f691a63..0000000000 --- a/ros2/src/airsim_ros_pkgs/include/utils.h +++ /dev/null @@ -1,14 +0,0 @@ -#include -namespace utils -{ -inline double get_yaw_from_quat_msg(const geometry_msgs::msg::Quaternion& quat_msg) -{ - tf2::Quaternion quat_tf; - double roll, pitch, yaw; - tf2::fromMsg(quat_msg, quat_tf); - tf2::Matrix3x3(quat_tf).getRPY(roll, pitch, yaw); - return yaw; -} - -// OdometryEuler get_eigen_odom_from_rosmsg(const nav::msgs &odom_msg); -} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py b/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py deleted file mode 100644 index 8343f596ee..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/airsim_node.launch.py +++ /dev/null @@ -1,62 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch.launch_description_sources import PythonLaunchDescriptionSource - -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - - output = DeclareLaunchArgument( - "output", - default_value='log') - - publish_clock = DeclareLaunchArgument( - "publish_clock", - default_value='False') - - is_vulkan = DeclareLaunchArgument( - "is_vulkan", - default_value='True') - - host = DeclareLaunchArgument( - "host", - default_value='localhost') - - airsim_node = Node( - package='airsim_ros_pkgs', - executable='airsim_node', - name='airsim_node', - output='screen', - parameters=[{ - 'is_vulkan': False, - 'update_airsim_img_response_every_n_sec': 0.05, - 'update_airsim_control_every_n_sec': 0.01, - 'update_lidar_every_n_sec': 0.01, - 'publish_clock': LaunchConfiguration('publish_clock'), - 'host_ip': LaunchConfiguration('host') - }]) - - static_transforms = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory('airsim_ros_pkgs'), 'launch/static_transforms.launch.py') - ) - ) - - # Create the launch description and populate - ld = LaunchDescription() - - # Declare the launch options - ld.add_action(output) - ld.add_action(publish_clock) - ld.add_action(is_vulkan) - ld.add_action(host) - - ld.add_action(static_transforms) - ld.add_action(airsim_node) - - return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py b/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py deleted file mode 100644 index 55ce609470..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/airsim_with_simple_PD_position_controller.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - ld = LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'airsim_ros_pkgs'), 'launch/airsim_node.launch.py') - ) - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'airsim_ros_pkgs'), 'launch/dynamic_constraints.launch.py') - ) - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(get_package_share_directory( - 'airsim_ros_pkgs'), 'launch/position_controller_simple.launch.py') - ) - ) - ]) - return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py b/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py deleted file mode 100644 index d8449582df..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/dynamic_constraints.launch.py +++ /dev/null @@ -1,46 +0,0 @@ - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument - -def generate_launch_description(): - ld = LaunchDescription([ - - DeclareLaunchArgument( - "max_vel_vert_abs", - default_value='10.0'), - - DeclareLaunchArgument( - "max_vel_horz_abs", - default_value='0.5'), - - DeclareLaunchArgument( - "max_yaw_rate_degree", - default_value='1.0'), - - DeclareLaunchArgument( - "gimbal_front_center_max_pitch", - default_value='40.0'), - - DeclareLaunchArgument( - "gimbal_front_center_min_pitch", - default_value='-130.0'), - - DeclareLaunchArgument( - "gimbal_front_center_max_yaw", - default_value='320.0'), - - DeclareLaunchArgument( - "gimbal_front_center_min_yaw", - default_value='-320.0'), - - DeclareLaunchArgument( - "gimbal_front_center_min_yaw", - default_value='20.0'), - - DeclareLaunchArgument( - "gimbal_front_center_min_yaw", - default_value='-20.0') - - ]) - - return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py b/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py deleted file mode 100644 index 036d87c061..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/position_controller_simple.launch.py +++ /dev/null @@ -1,32 +0,0 @@ -import launch -from launch_ros.actions import Node - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - Node( - package='airsim_ros_pkgs', - executable='pd_position_controller_simple_node', - name='pid_position_node', - output='screen', - parameters=[{ - 'update_control_every_n_sec': 0.01, - - 'kp_x': 0.30, - 'kp_y': 0.30, - 'kp_z': 0.30, - 'kp_yaw': 0.30, - - 'kd_x': 0.05, - 'kd_y': 0.05, - 'kd_z': 0.05, - 'kd_yaw': 0.05, - - 'reached_thresh_xyz': 0.1, - 'reached_yaw_degrees': 5.0 - } - ] - ) - ]) - - return ld diff --git a/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py b/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py deleted file mode 100644 index 19a83d48fe..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/rviz.launch.py +++ /dev/null @@ -1,21 +0,0 @@ -import os - -import launch -import launch_ros.actions -from ament_index_python.packages import get_package_share_directory - - -def generate_launch_description(): - - pkg_share = get_package_share_directory('airsim_ros_pkgs') - default_rviz_path = os.path.join(pkg_share, 'rviz/default.rviz') - - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='rviz2', - executable='rviz2', - name='rviz2', - arguments=['-d', default_rviz_path] - ) - ]) - return ld \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py b/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py deleted file mode 100644 index 420137977d..0000000000 --- a/ros2/src/airsim_ros_pkgs/launch/static_transforms.launch.py +++ /dev/null @@ -1,15 +0,0 @@ - -import launch -import launch_ros.actions - - -def generate_launch_description(): - ld = launch.LaunchDescription([ - launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - name='ned_to_enu_pub', - arguments=['0', '0', '0', '1.57', '0', '3.14', 'world_ned', 'world_enu']#, '100'] - ) - ]) - return ld diff --git a/ros2/src/airsim_ros_pkgs/package.xml b/ros2/src/airsim_ros_pkgs/package.xml deleted file mode 100644 index 74d1f44f20..0000000000 --- a/ros2/src/airsim_ros_pkgs/package.xml +++ /dev/null @@ -1,53 +0,0 @@ - - - airsim_ros_pkgs - 0.0.1 - ROS Wrapper over AirSim's C++ client library - - Ratnesh Madaan - - MIT - - ament_cmake - - rosidl_interface_packages - - geometry_msgs - image_transport - message_runtime - mavros_msgs - nav_msgs - rclcpp - rclpy - sensor_msgs - std_msgs - geographic_msgs - geometry_msgs - tf2_sensor_msgs - - airsim_interfaces - - builtin_interfaces - builtin_interfaces - rosidl_default_generators - - geometry_msgs - image_transport - message_generation - message_runtime - nav_msgs - rclcpp - rclpy - sensor_msgs - std_msgs - joy - - rosidl_default_generators - rosidl_default_runtime - - - ament_cmake - - - ament_cmake - diff --git a/ros2/src/airsim_ros_pkgs/rviz/default.rviz b/ros2/src/airsim_ros_pkgs/rviz/default.rviz deleted file mode 100755 index 7f68da5d14..0000000000 --- a/ros2/src/airsim_ros_pkgs/rviz/default.rviz +++ /dev/null @@ -1,276 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 78 - Name: Displays - Property Tree Widget: - Expanded: - - /TF1 - - /TF1/Frames1 - - /Odometry1 - - /Odometry1/Shape1 - - /PointCloud21 - Splitter Ratio: 0.5051546096801758 - Tree Height: 635 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - Name: Time - Splitter Ratio: 0.5 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz_default_plugins/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz_default_plugins/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - SimpleFlight: - Value: true - SimpleFlight/LidarCustom: - Value: true - SimpleFlight/odom_local_ned: - Value: true - front_camera_body: - Value: true - front_camera_body/static: - Value: true - front_camera_optical: - Value: true - front_camera_optical/static: - Value: true - world_enu: - Value: false - world_ned: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - world_ned: - SimpleFlight: - SimpleFlight/odom_local_ned: - SimpleFlight/LidarCustom: - {} - front_camera_body/static: - {} - front_camera_optical/static: - {} - front_camera_body: - {} - front_camera_optical: - {} - world_enu: - {} - Update Interval: 0 - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: RGB8 - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: front_left_depth_planar_registered_point_cloud - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.10000000149011612 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /airsim_node/front_left_custom/DepthPlanar/registered/points - Use Fixed Frame: true - Use rainbow: true - Value: true - - Angle Tolerance: 0.10000000149011612 - Class: rviz_default_plugins/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 25 - Name: Odometry - Position Tolerance: 0.10000000149011612 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.20000000298023224 - Head Radius: 0.05000000074505806 - Shaft Length: 0.20000000298023224 - Shaft Radius: 0.019999999552965164 - Value: Arrow - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /airsim_node/MyQuad/odom_local_ned - Value: true - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 4096 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: PointCloud2 - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /airsim_node/drone_1/lidar/LidarCustom - Use Fixed Frame: true - Use rainbow: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: world_enu - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /move_base_simple/goal - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 10 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0 - Y: 0 - Z: 0 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398006439209 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 864 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 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 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Width: 1846 - X: 72 - Y: 27 diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp deleted file mode 100644 index 1003f8556a..0000000000 --- a/ros2/src/airsim_ros_pkgs/src/airsim_node.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include -#include "airsim_ros_wrapper.h" - -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::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); - - 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 deleted file mode 100755 index 0d8aeb0328..0000000000 --- a/ros2/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ /dev/null @@ -1,1441 +0,0 @@ -#include -#include "common/AirSimSettings.hpp" -#include - -using namespace std::placeholders; - -constexpr char AirsimROSWrapper::CAM_YML_NAME[]; -constexpr char AirsimROSWrapper::WIDTH_YML_NAME[]; -constexpr char AirsimROSWrapper::HEIGHT_YML_NAME[]; -constexpr char AirsimROSWrapper::K_YML_NAME[]; -constexpr char AirsimROSWrapper::D_YML_NAME[]; -constexpr char AirsimROSWrapper::R_YML_NAME[]; -constexpr char AirsimROSWrapper::P_YML_NAME[]; -constexpr char AirsimROSWrapper::DMODEL_YML_NAME[]; - -const std::unordered_map AirsimROSWrapper::image_type_int_to_string_map_ = { - { 0, "Scene" }, - { 1, "DepthPlanar" }, - { 2, "DepthPerspective" }, - { 3, "DepthVis" }, - { 4, "DisparityNormalized" }, - { 5, "Segmentation" }, - { 6, "SurfaceNormals" }, - { 7, "Infrared" } -}; - -AirsimROSWrapper::AirsimROSWrapper(const std::shared_ptr nh, const std::shared_ptr nh_img, const std::shared_ptr nh_lidar, const std::string& host_ip) - : is_used_lidar_timer_cb_queue_(false) - , is_used_img_timer_cb_queue_(false) - , airsim_settings_parser_(host_ip) - , host_ip_(host_ip) - , airsim_client_(nullptr) - , airsim_client_images_(host_ip) - , airsim_client_lidar_(host_ip) - , nh_(nh) - , nh_img_(nh_img) - , nh_lidar_(nh_lidar) - , isENU_(false) - , publish_clock_(false) -{ - ros_clock_.clock = rclcpp::Time(0); - - if (AirSimSettings::singleton().simmode_name != AirSimSettings::kSimModeTypeCar) { - airsim_mode_ = AIRSIM_MODE::DRONE; - RCLCPP_INFO(nh_->get_logger(), "Setting ROS wrapper to DRONE mode"); - } - else { - airsim_mode_ = AIRSIM_MODE::CAR; - RCLCPP_INFO(nh_->get_logger(), "Setting ROS wrapper to CAR mode"); - } - tf_buffer_ = std::make_shared(nh_->get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - tf_broadcaster_ = std::make_shared(nh_); - static_tf_pub_ = std::make_shared(nh_); - - initialize_ros(); - - RCLCPP_INFO(nh_->get_logger(), "AirsimROSWrapper Initialized!"); -} - -void AirsimROSWrapper::initialize_airsim() -{ - // todo do not reset if already in air? - try { - - if (airsim_mode_ == AIRSIM_MODE::DRONE) { - airsim_client_ = std::unique_ptr(new msr::airlib::MultirotorRpcLibClient(host_ip_)); - } - else { - airsim_client_ = std::unique_ptr(new msr::airlib::CarRpcLibClient(host_ip_)); - } - airsim_client_->confirmConnection(); - airsim_client_images_.confirmConnection(); - airsim_client_lidar_.confirmConnection(); - - for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - airsim_client_->enableApiControl(true, vehicle_name_ptr_pair.first); // todo expose as rosservice? - airsim_client_->armDisarm(true, vehicle_name_ptr_pair.first); // todo exposes as rosservice? - } - - origin_geo_point_ = get_origin_geo_point(); - // todo there's only one global origin geopoint for environment. but airsim API accept a parameter vehicle_name? inside carsimpawnapi.cpp, there's a geopoint being assigned in the constructor. by? - origin_geo_point_msg_ = get_gps_msg_from_airsim_geo_point(origin_geo_point_); - } - catch (rpc::rpc_error& e) { - std::string msg = e.get_error().as(); - RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, something went wrong.\n%s", msg); - rclcpp::shutdown(); - } -} - -void AirsimROSWrapper::initialize_ros() -{ - - // ros params - double update_airsim_control_every_n_sec; - nh_->get_parameter("is_vulkan", is_vulkan_); - nh_->get_parameter("update_airsim_control_every_n_sec", update_airsim_control_every_n_sec); - nh_->get_parameter("publish_clock", publish_clock_); - nh_->get_parameter_or("world_frame_id", world_frame_id_, world_frame_id_); - odom_frame_id_ = world_frame_id_ == AIRSIM_FRAME_ID ? AIRSIM_ODOM_FRAME_ID : ENU_ODOM_FRAME_ID; - nh_->get_parameter_or("odom_frame_id", odom_frame_id_, odom_frame_id_); - isENU_ = (odom_frame_id_ == ENU_ODOM_FRAME_ID); - nh_->get_parameter_or("coordinate_system_enu", isENU_, isENU_); - vel_cmd_duration_ = 0.05; // todo rosparam - // todo enforce dynamics constraints in this node as well? - // nh_->get_parameter("max_vert_vel_", max_vert_vel_); - // nh_->get_parameter("max_horz_vel", max_horz_vel_) - - 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)); -} - -void AirsimROSWrapper::create_ros_pubs_from_settings_json() -{ - // subscribe to control commands on global nodehandle - gimbal_angle_quat_cmd_sub_ = nh_->create_subscription("~/gimbal_angle_quat_cmd", 50, std::bind(&AirsimROSWrapper::gimbal_angle_quat_cmd_cb, this, _1)); - gimbal_angle_euler_cmd_sub_ = nh_->create_subscription("~/gimbal_angle_euler_cmd", 50, std::bind(&AirsimROSWrapper::gimbal_angle_euler_cmd_cb, this, _1)); - origin_geo_point_pub_ = nh_->create_publisher("~/origin_geo_point", 10); - - airsim_img_request_vehicle_name_pair_vec_.clear(); - image_pub_vec_.clear(); - cam_info_pub_vec_.clear(); - camera_info_msg_vec_.clear(); - vehicle_name_ptr_map_.clear(); - size_t lidar_cnt = 0; - - image_transport::ImageTransport image_transporter(nh_); - - // iterate over std::map> vehicles; - for (const auto& curr_vehicle_elem : AirSimSettings::singleton().vehicles) { - auto& vehicle_setting = curr_vehicle_elem.second; - auto curr_vehicle_name = curr_vehicle_elem.first; - - nh_->set_parameter(rclcpp::Parameter("vehicle_name", curr_vehicle_name)); - - set_nans_to_zeros_in_pose(*vehicle_setting); - - std::unique_ptr vehicle_ros = nullptr; - - if (airsim_mode_ == AIRSIM_MODE::DRONE) { - vehicle_ros = std::unique_ptr(new MultiRotorROS()); - } - else { - vehicle_ros = std::unique_ptr(new CarROS()); - } - - vehicle_ros->odom_frame_id_ = curr_vehicle_name + "/" + odom_frame_id_; - vehicle_ros->vehicle_name_ = curr_vehicle_name; - - append_static_vehicle_tf(vehicle_ros.get(), *vehicle_setting); - - const std::string topic_prefix = "~/" + curr_vehicle_name; - vehicle_ros->odom_local_pub_ = nh_->create_publisher(topic_prefix + "/" + odom_frame_id_, 10); - - vehicle_ros->env_pub_ = nh_->create_publisher(topic_prefix + "/environment", 10); - - vehicle_ros->global_gps_pub_ = nh_->create_publisher(topic_prefix + "/global_gps", 10); - - if (airsim_mode_ == AIRSIM_MODE::DRONE) { - auto drone = static_cast(vehicle_ros.get()); - - // bind to a single callback. todo optimal subs queue length - // bind multiple topics to a single callback, but keep track of which vehicle name it was by passing curr_vehicle_name as the 2nd argument - - std::function fcn_vel_cmd_body_frame_sub = std::bind(&AirsimROSWrapper::vel_cmd_body_frame_cb, this, _1, vehicle_ros->vehicle_name_); - drone->vel_cmd_body_frame_sub_ = nh_->create_subscription(topic_prefix + "/vel_cmd_body_frame", 1, fcn_vel_cmd_body_frame_sub); // todo ros::TransportHints().tcpNoDelay(); - - std::function fcn_vel_cmd_world_frame_sub = std::bind(&AirsimROSWrapper::vel_cmd_world_frame_cb, this, _1, vehicle_ros->vehicle_name_); - drone->vel_cmd_world_frame_sub_ = nh_->create_subscription(topic_prefix + "/vel_cmd_world_frame", 1, fcn_vel_cmd_world_frame_sub); - - std::function, std::shared_ptr)> fcn_takeoff_srvr = std::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, vehicle_ros->vehicle_name_); - drone->takeoff_srvr_ = nh_->create_service(topic_prefix + "/takeoff", fcn_takeoff_srvr); - - std::function, std::shared_ptr)> fcn_land_srvr = std::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, vehicle_ros->vehicle_name_); - drone->land_srvr_ = nh_->create_service(topic_prefix + "/land", fcn_land_srvr); - - // vehicle_ros.reset_srvr = nh_->create_service(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); - } - else { - auto car = static_cast(vehicle_ros.get()); - std::function fcn_car_cmd_sub = std::bind(&AirsimROSWrapper::car_cmd_cb, this, _1, vehicle_ros->vehicle_name_); - car->car_cmd_sub_ = nh_->create_subscription(topic_prefix + "/car_cmd", 1, fcn_car_cmd_sub); - car->car_state_pub_ = nh_->create_publisher(topic_prefix + "/car_state", 10); - } - - // iterate over camera map std::map .cameras; - for (auto& curr_camera_elem : vehicle_setting->cameras) { - auto& camera_setting = curr_camera_elem.second; - auto& curr_camera_name = curr_camera_elem.first; - - set_nans_to_zeros_in_pose(*vehicle_setting, camera_setting); - append_static_camera_tf(vehicle_ros.get(), curr_camera_name, camera_setting); - // camera_setting.gimbal - std::vector current_image_request_vec; - current_image_request_vec.clear(); - - // iterate over capture_setting std::map capture_settings - for (const auto& curr_capture_elem : camera_setting.capture_settings) { - auto& capture_setting = curr_capture_elem.second; - - // todo why does AirSimSettings::loadCaptureSettings calls AirSimSettings::initializeCaptureSettings() - // which initializes default capture settings for _all_ NINE msr::airlib::ImageCaptureBase::ImageType - if (!(std::isnan(capture_setting.fov_degrees))) { - ImageType curr_image_type = msr::airlib::Utils::toEnum(capture_setting.image_type); - // if scene / segmentation / surface normals / infrared, get uncompressed image with pixels_as_floats = false - if (curr_image_type == ImageType::Scene || curr_image_type == ImageType::Segmentation || curr_image_type == ImageType::SurfaceNormals || curr_image_type == ImageType::Infrared) { - current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, false, false)); - } - // if {DepthPlanar, DepthPerspective,DepthVis, DisparityNormalized}, get float image - else { - current_image_request_vec.push_back(ImageRequest(curr_camera_name, curr_image_type, true)); - } - - const std::string camera_topic = topic_prefix + "/" + curr_camera_name + "/" + image_type_int_to_string_map_.at(capture_setting.image_type); - image_pub_vec_.push_back(image_transporter.advertise(camera_topic, 1)); - cam_info_pub_vec_.push_back(nh_->create_publisher(camera_topic + "/camera_info", 10)); - camera_info_msg_vec_.push_back(generate_cam_info(curr_camera_name, camera_setting, capture_setting)); - } - } - // push back pair (vector of image captures, current vehicle name) - airsim_img_request_vehicle_name_pair_vec_.push_back(std::make_pair(current_image_request_vec, curr_vehicle_name)); - } - - // iterate over sensors - for (auto& curr_sensor_map : vehicle_setting->sensors) { - auto& sensor_name = curr_sensor_map.first; - auto& sensor_setting = curr_sensor_map.second; - - if (sensor_setting->enabled) { - - switch (sensor_setting->sensor_type) { - case SensorBase::SensorType::Barometer: { - SensorPublisher sensor_publisher = - create_sensor_publisher("Barometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/altimeter/" + sensor_name, 10); - vehicle_ros->barometer_pubs_.emplace_back(sensor_publisher); - break; - } - case SensorBase::SensorType::Imu: { - SensorPublisher sensor_publisher = - create_sensor_publisher("Imu", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/imu/" + sensor_name, 10); - vehicle_ros->imu_pubs_.emplace_back(sensor_publisher); - break; - } - case SensorBase::SensorType::Gps: { - SensorPublisher sensor_publisher = - create_sensor_publisher("Gps", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/gps/" + sensor_name, 10); - vehicle_ros->gps_pubs_.emplace_back(sensor_publisher); - break; - } - case SensorBase::SensorType::Magnetometer: { - SensorPublisher sensor_publisher = - create_sensor_publisher("Magnetometer", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/magnetometer/" + sensor_name, 10); - vehicle_ros->magnetometer_pubs_.emplace_back(sensor_publisher); - break; - } - case SensorBase::SensorType::Distance: { - SensorPublisher sensor_publisher = - create_sensor_publisher("Distance", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/distance/" + sensor_name, 10); - vehicle_ros->distance_pubs_.emplace_back(sensor_publisher); - break; - } - case SensorBase::SensorType::Lidar: { - auto lidar_setting = *static_cast(sensor_setting.get()); - msr::airlib::LidarSimpleParams params; - params.initializeFromSettings(lidar_setting); - append_static_lidar_tf(vehicle_ros.get(), sensor_name, params); - - SensorPublisher sensor_publisher = - create_sensor_publisher("Lidar", sensor_setting->sensor_name, sensor_setting->sensor_type, curr_vehicle_name + "/lidar/" + sensor_name, 10); - vehicle_ros->lidar_pubs_.emplace_back(sensor_publisher); - lidar_cnt += 1; - break; - } - default: { - throw std::invalid_argument("Unexpected sensor type"); - } - } - } - } - - vehicle_name_ptr_map_.emplace(curr_vehicle_name, std::move(vehicle_ros)); // allows fast lookup in command callbacks in case of a lot of drones - } - - // add takeoff and land all services if more than 2 drones - if (vehicle_name_ptr_map_.size() > 1 && airsim_mode_ == AIRSIM_MODE::DRONE) { - takeoff_all_srvr_ = nh_->create_service("~/all_robots/takeoff", std::bind(&AirsimROSWrapper::takeoff_all_srv_cb, this, _1, _2)); - land_all_srvr_ = nh_->create_service("~/all_robots/land", std::bind(&AirsimROSWrapper::land_all_srv_cb, this, _1, _2)); - - vel_cmd_all_body_frame_sub_ = nh_->create_subscription("~/all_robots/vel_cmd_body_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_all_body_frame_cb, this, _1)); - vel_cmd_all_world_frame_sub_ = nh_->create_subscription("~/all_robots/vel_cmd_world_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_all_world_frame_cb, this, _1)); - - vel_cmd_group_body_frame_sub_ = nh_->create_subscription("~/group_of_robots/vel_cmd_body_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_group_body_frame_cb, this, _1)); - vel_cmd_group_world_frame_sub_ = nh_->create_subscription("~/group_of_robots/vel_cmd_world_frame", 1, std::bind(&AirsimROSWrapper::vel_cmd_group_world_frame_cb, this, _1)); - - takeoff_group_srvr_ = nh_->create_service("~/group_of_robots/takeoff", std::bind(&AirsimROSWrapper::takeoff_group_srv_cb, this, _1, _2)); - land_group_srvr_ = nh_->create_service("~/group_of_robots/land", std::bind(&AirsimROSWrapper::land_group_srv_cb, this, _1, _2)); - } - - // todo add per vehicle reset in AirLib API - reset_srvr_ = nh_->create_service("~/reset", std::bind(&AirsimROSWrapper::reset_srv_cb, this, _1, _2)); - - if (publish_clock_) { - clock_pub_ = nh_->create_publisher("~/clock", 1); - } - - // if >0 cameras, add one more thread for img_request_timer_cb - 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)); - is_used_img_timer_cb_queue_ = true; - } - - // lidars update on their own callback/thread at a given rate - 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)); - is_used_lidar_timer_cb_queue_ = true; - } - - initialize_airsim(); -} - -// QoS - The depth of the publisher message queue. -// more details here - https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html -template -const SensorPublisher AirsimROSWrapper::create_sensor_publisher(const string& sensor_type_name, const string& sensor_name, SensorBase::SensorType sensor_type, const string& topic_name, int QoS) -{ - RCLCPP_INFO_STREAM(nh_->get_logger(), sensor_type_name); - SensorPublisher sensor_publisher; - sensor_publisher.sensor_name = sensor_name; - sensor_publisher.sensor_type = sensor_type; - sensor_publisher.publisher = nh_->create_publisher("~/" + topic_name, QoS); - return sensor_publisher; -} - -// todo: error check. if state is not landed, return error. -bool AirsimROSWrapper::takeoff_srv_cb(std::shared_ptr request, std::shared_ptr response, const std::string& vehicle_name) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? - // response->success = - else - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); - // response->success = - - return true; -} - -bool AirsimROSWrapper::takeoff_group_srv_cb(std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - for (const auto& vehicle_name : request->vehicle_names) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? - // response->success = - else - for (const auto& vehicle_name : request->vehicle_names) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); - // response->success = - - return true; -} - -bool AirsimROSWrapper::takeoff_all_srv_cb(std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first)->waitOnLastTask(); // todo value for timeout_sec? - // response->success = - else - for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name_ptr_pair.first); - // response->success = - - return true; -} - -bool AirsimROSWrapper::land_srv_cb(std::shared_ptr request, std::shared_ptr response, const std::string& vehicle_name) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); - else - static_cast(airsim_client_.get())->landAsync(60, vehicle_name); - - return true; //todo -} - -bool AirsimROSWrapper::land_group_srv_cb(std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - for (const auto& vehicle_name : request->vehicle_names) - static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); - else - for (const auto& vehicle_name : request->vehicle_names) - static_cast(airsim_client_.get())->landAsync(60, vehicle_name); - - return true; //todo -} - -bool AirsimROSWrapper::land_all_srv_cb(std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - std::lock_guard guard(control_mutex_); - - if (request->wait_on_last_task) - for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first)->waitOnLastTask(); - else - for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) - static_cast(airsim_client_.get())->landAsync(60, vehicle_name_ptr_pair.first); - - return true; //todo -} - -// todo add reset by vehicle_name API to airlib -// todo not async remove wait_on_last_task -bool AirsimROSWrapper::reset_srv_cb(std::shared_ptr request, std::shared_ptr response) -{ - unused(request); - unused(response); - std::lock_guard guard(control_mutex_); - - airsim_client_.reset(); - return true; //todo -} - -tf2::Quaternion AirsimROSWrapper::get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const -{ - return tf2::Quaternion(airlib_quat.x(), airlib_quat.y(), airlib_quat.z(), airlib_quat.w()); -} - -msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const geometry_msgs::msg::Quaternion& geometry_msgs_quat) const -{ - return msr::airlib::Quaternionr(geometry_msgs_quat.w, geometry_msgs_quat.x, geometry_msgs_quat.y, geometry_msgs_quat.z); -} - -msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion& tf2_quat) const -{ - return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); -} - -void AirsimROSWrapper::car_cmd_cb(const airsim_interfaces::msg::CarControls::SharedPtr msg, const std::string& vehicle_name) -{ - std::lock_guard guard(control_mutex_); - - auto car = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - car->car_cmd_.throttle = msg->throttle; - car->car_cmd_.steering = msg->steering; - car->car_cmd_.brake = msg->brake; - car->car_cmd_.handbrake = msg->handbrake; - car->car_cmd_.is_manual_gear = msg->manual; - car->car_cmd_.manual_gear = msg->manual_gear; - car->car_cmd_.gear_immediate = msg->gear_immediate; - - car->has_car_cmd_ = true; -} - -msr::airlib::Pose AirsimROSWrapper::get_airlib_pose(const float& x, const float& y, const float& z, const msr::airlib::Quaternionr& airlib_quat) const -{ - return msr::airlib::Pose(msr::airlib::Vector3r(x, y, z), airlib_quat); -} - -void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name) -{ - std::lock_guard guard(control_mutex_); - - auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - drone->vel_cmd_ = get_airlib_body_vel_cmd(*msg, drone->curr_drone_state_.kinematics_estimated.pose.orientation); - drone->has_vel_cmd_ = true; -} - -void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg) -{ - std::lock_guard guard(control_mutex_); - - for (const auto& vehicle_name : msg->vehicle_names) { - auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - drone->vel_cmd_ = get_airlib_body_vel_cmd(msg->vel_cmd, drone->curr_drone_state_.kinematics_estimated.pose.orientation); - drone->has_vel_cmd_ = true; - } -} - -void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg) -{ - std::lock_guard guard(control_mutex_); - - // todo expose wait_on_last_task or nah? - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - auto drone = static_cast(vehicle_name_ptr_pair.second.get()); - drone->vel_cmd_ = get_airlib_body_vel_cmd(*msg, drone->curr_drone_state_.kinematics_estimated.pose.orientation); - drone->has_vel_cmd_ = true; - } -} - -void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg, const std::string& vehicle_name) -{ - std::lock_guard guard(control_mutex_); - - auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - drone->vel_cmd_ = get_airlib_world_vel_cmd(*msg); - drone->has_vel_cmd_ = true; -} - -// this is kinda unnecessary but maybe it makes life easier for the end user. -void AirsimROSWrapper::vel_cmd_group_world_frame_cb(const airsim_interfaces::msg::VelCmdGroup::SharedPtr msg) -{ - std::lock_guard guard(control_mutex_); - - for (const auto& vehicle_name : msg->vehicle_names) { - auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - drone->vel_cmd_ = get_airlib_world_vel_cmd(msg->vel_cmd); - drone->has_vel_cmd_ = true; - } -} - -void AirsimROSWrapper::vel_cmd_all_world_frame_cb(const airsim_interfaces::msg::VelCmd::SharedPtr msg) -{ - std::lock_guard guard(control_mutex_); - - // todo expose wait_on_last_task or nah? - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - auto drone = static_cast(vehicle_name_ptr_pair.second.get()); - drone->vel_cmd_ = get_airlib_world_vel_cmd(*msg); - drone->has_vel_cmd_ = true; - } -} - -// todo support multiple gimbal commands -void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_interfaces::msg::GimbalAngleQuatCmd::SharedPtr gimbal_angle_quat_cmd_msg) -{ - tf2::Quaternion quat_control_cmd; - try { - tf2::convert(gimbal_angle_quat_cmd_msg->orientation, quat_control_cmd); - quat_control_cmd.normalize(); - gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); // airsim uses wxyz - gimbal_cmd_.camera_name = gimbal_angle_quat_cmd_msg->camera_name; - gimbal_cmd_.vehicle_name = gimbal_angle_quat_cmd_msg->vehicle_name; - has_gimbal_cmd_ = true; - } - catch (tf2::TransformException& ex) { - RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); - } -} - -// todo support multiple gimbal commands -// 1. find quaternion of default gimbal pose -// 2. forward multiply with quaternion equivalent to desired euler commands (in degrees) -// 3. call airsim client's setCameraPose which sets camera pose wrt world (or takeoff?) ned frame. todo -void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_interfaces::msg::GimbalAngleEulerCmd::SharedPtr gimbal_angle_euler_cmd_msg) -{ - try { - tf2::Quaternion quat_control_cmd; - quat_control_cmd.setRPY(math_common::deg2rad(gimbal_angle_euler_cmd_msg->roll), math_common::deg2rad(gimbal_angle_euler_cmd_msg->pitch), math_common::deg2rad(gimbal_angle_euler_cmd_msg->yaw)); - quat_control_cmd.normalize(); - gimbal_cmd_.target_quat = get_airlib_quat(quat_control_cmd); - gimbal_cmd_.camera_name = gimbal_angle_euler_cmd_msg->camera_name; - gimbal_cmd_.vehicle_name = gimbal_angle_euler_cmd_msg->vehicle_name; - has_gimbal_cmd_ = true; - } - catch (tf2::TransformException& ex) { - RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); - } -} - -airsim_interfaces::msg::CarState AirsimROSWrapper::get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const -{ - airsim_interfaces::msg::CarState state_msg; - const auto odo = get_odom_msg_from_car_state(car_state); - - state_msg.pose = odo.pose; - state_msg.twist = odo.twist; - state_msg.speed = car_state.speed; - state_msg.gear = car_state.gear; - state_msg.rpm = car_state.rpm; - state_msg.maxrpm = car_state.maxrpm; - state_msg.handbrake = car_state.handbrake; - state_msg.header.stamp = airsim_timestamp_to_ros(car_state.timestamp); - - return state_msg; -} - -nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_kinematic_state(const msr::airlib::Kinematics::State& kinematics_estimated) const -{ - nav_msgs::msg::Odometry odom_msg; - - odom_msg.pose.pose.position.x = kinematics_estimated.pose.position.x(); - odom_msg.pose.pose.position.y = kinematics_estimated.pose.position.y(); - odom_msg.pose.pose.position.z = kinematics_estimated.pose.position.z(); - odom_msg.pose.pose.orientation.x = kinematics_estimated.pose.orientation.x(); - odom_msg.pose.pose.orientation.y = kinematics_estimated.pose.orientation.y(); - odom_msg.pose.pose.orientation.z = kinematics_estimated.pose.orientation.z(); - odom_msg.pose.pose.orientation.w = kinematics_estimated.pose.orientation.w(); - - odom_msg.twist.twist.linear.x = kinematics_estimated.twist.linear.x(); - odom_msg.twist.twist.linear.y = kinematics_estimated.twist.linear.y(); - odom_msg.twist.twist.linear.z = kinematics_estimated.twist.linear.z(); - odom_msg.twist.twist.angular.x = kinematics_estimated.twist.angular.x(); - odom_msg.twist.twist.angular.y = kinematics_estimated.twist.angular.y(); - odom_msg.twist.twist.angular.z = kinematics_estimated.twist.angular.z(); - - if (isENU_) { - std::swap(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y); - odom_msg.pose.pose.position.z = -odom_msg.pose.pose.position.z; - std::swap(odom_msg.pose.pose.orientation.x, odom_msg.pose.pose.orientation.y); - odom_msg.pose.pose.orientation.z = -odom_msg.pose.pose.orientation.z; - std::swap(odom_msg.twist.twist.linear.x, odom_msg.twist.twist.linear.y); - odom_msg.twist.twist.linear.z = -odom_msg.twist.twist.linear.z; - std::swap(odom_msg.twist.twist.angular.x, odom_msg.twist.twist.angular.y); - odom_msg.twist.twist.angular.z = -odom_msg.twist.twist.angular.z; - } - - return odom_msg; -} - -nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const -{ - return get_odom_msg_from_kinematic_state(car_state.kinematics_estimated); -} - -nav_msgs::msg::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const -{ - return get_odom_msg_from_kinematic_state(drone_state.kinematics_estimated); -} - -// https://docs.ros.org/jade/api/sensor_msgs/html/point__cloud__conversion_8h_source.html#l00066 -// look at UnrealLidarSensor.cpp UnrealLidarSensor::getPointCloud() for math -// read this carefully https://docs.ros.org/kinetic/api/sensor_msgs/html/msg/PointCloud2.html -sensor_msgs::msg::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const -{ - sensor_msgs::msg::PointCloud2 lidar_msg; - lidar_msg.header.stamp = nh_->now(); - lidar_msg.header.frame_id = vehicle_name; - - if (lidar_data.point_cloud.size() > 3) { - lidar_msg.height = 1; - lidar_msg.width = lidar_data.point_cloud.size() / 3; - - lidar_msg.fields.resize(3); - lidar_msg.fields[0].name = "x"; - lidar_msg.fields[1].name = "y"; - lidar_msg.fields[2].name = "z"; - - int offset = 0; - - for (size_t d = 0; d < lidar_msg.fields.size(); ++d, offset += 4) { - lidar_msg.fields[d].offset = offset; - lidar_msg.fields[d].datatype = sensor_msgs::msg::PointField::FLOAT32; - lidar_msg.fields[d].count = 1; - } - - lidar_msg.is_bigendian = false; - lidar_msg.point_step = offset; // 4 * num fields - lidar_msg.row_step = lidar_msg.point_step * lidar_msg.width; - - lidar_msg.is_dense = true; // todo - std::vector data_std = lidar_data.point_cloud; - - const unsigned char* bytes = reinterpret_cast(data_std.data()); - vector lidar_msg_data(bytes, bytes + sizeof(float) * data_std.size()); - lidar_msg.data = std::move(lidar_msg_data); - } - else { - // msg = [] - } - - if (isENU_) { - try { - sensor_msgs::msg::PointCloud2 lidar_msg_enu; - auto transformStampedENU = tf_buffer_->lookupTransform(AIRSIM_FRAME_ID, vehicle_name, rclcpp::Time(0), rclcpp::Duration(1)); - tf2::doTransform(lidar_msg, lidar_msg_enu, transformStampedENU); - - lidar_msg_enu.header.stamp = lidar_msg.header.stamp; - lidar_msg_enu.header.frame_id = lidar_msg.header.frame_id; - - lidar_msg = std::move(lidar_msg_enu); - } - catch (tf2::TransformException& ex) { - RCLCPP_WARN(nh_->get_logger(), "%s", ex.what()); - rclcpp::Rate(1.0).sleep(); - } - } - - return lidar_msg; -} - -airsim_interfaces::msg::Environment AirsimROSWrapper::get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const -{ - airsim_interfaces::msg::Environment env_msg; - env_msg.position.x = env_data.position.x(); - env_msg.position.y = env_data.position.y(); - env_msg.position.z = env_data.position.z(); - env_msg.geo_point.latitude = env_data.geo_point.latitude; - env_msg.geo_point.longitude = env_data.geo_point.longitude; - env_msg.geo_point.altitude = env_data.geo_point.altitude; - env_msg.gravity.x = env_data.gravity.x(); - env_msg.gravity.y = env_data.gravity.y(); - env_msg.gravity.z = env_data.gravity.z(); - env_msg.air_pressure = env_data.air_pressure; - env_msg.temperature = env_data.temperature; - env_msg.air_density = env_data.temperature; - - return env_msg; -} - -sensor_msgs::msg::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const -{ - sensor_msgs::msg::MagneticField mag_msg; - mag_msg.magnetic_field.x = mag_data.magnetic_field_body.x(); - mag_msg.magnetic_field.y = mag_data.magnetic_field_body.y(); - mag_msg.magnetic_field.z = mag_data.magnetic_field_body.z(); - std::copy(std::begin(mag_data.magnetic_field_covariance), - std::end(mag_data.magnetic_field_covariance), - std::begin(mag_msg.magnetic_field_covariance)); - mag_msg.header.stamp = airsim_timestamp_to_ros(mag_data.time_stamp); - - return mag_msg; -} - -// todo covariances -sensor_msgs::msg::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const -{ - sensor_msgs::msg::NavSatFix gps_msg; - gps_msg.header.stamp = airsim_timestamp_to_ros(gps_data.time_stamp); - gps_msg.latitude = gps_data.gnss.geo_point.latitude; - gps_msg.longitude = gps_data.gnss.geo_point.longitude; - gps_msg.altitude = gps_data.gnss.geo_point.altitude; - gps_msg.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GLONASS; - gps_msg.status.status = gps_data.gnss.fix_type; - // gps_msg.position_covariance_type = - // gps_msg.position_covariance = - - return gps_msg; -} - -sensor_msgs::msg::Range AirsimROSWrapper::get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const -{ - sensor_msgs::msg::Range dist_msg; - dist_msg.header.stamp = airsim_timestamp_to_ros(dist_data.time_stamp); - dist_msg.range = dist_data.distance; - dist_msg.min_range = dist_data.min_distance; - dist_msg.max_range = dist_data.min_distance; - - return dist_msg; -} - -airsim_interfaces::msg::Altimeter AirsimROSWrapper::get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const -{ - airsim_interfaces::msg::Altimeter alt_msg; - alt_msg.header.stamp = airsim_timestamp_to_ros(alt_data.time_stamp); - alt_msg.altitude = alt_data.altitude; - alt_msg.pressure = alt_data.pressure; - alt_msg.qnh = alt_data.qnh; - - return alt_msg; -} - -// todo covariances -sensor_msgs::msg::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const -{ - sensor_msgs::msg::Imu imu_msg; - // imu_msg.header.frame_id = "/airsim/odom_local_ned";// todo multiple drones - imu_msg.header.stamp = airsim_timestamp_to_ros(imu_data.time_stamp); - imu_msg.orientation.x = imu_data.orientation.x(); - imu_msg.orientation.y = imu_data.orientation.y(); - imu_msg.orientation.z = imu_data.orientation.z(); - imu_msg.orientation.w = imu_data.orientation.w(); - - // todo radians per second - imu_msg.angular_velocity.x = imu_data.angular_velocity.x(); - imu_msg.angular_velocity.y = imu_data.angular_velocity.y(); - imu_msg.angular_velocity.z = imu_data.angular_velocity.z(); - - // meters/s2^m - imu_msg.linear_acceleration.x = imu_data.linear_acceleration.x(); - imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y(); - imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z(); - - // imu_msg.orientation_covariance = ; - // imu_msg.angular_velocity_covariance = ; - // imu_msg.linear_acceleration_covariance = ; - - return imu_msg; -} - -void AirsimROSWrapper::publish_odom_tf(const nav_msgs::msg::Odometry& odom_msg) -{ - geometry_msgs::msg::TransformStamped odom_tf; - odom_tf.header = odom_msg.header; - odom_tf.child_frame_id = odom_msg.child_frame_id; - odom_tf.transform.translation.x = odom_msg.pose.pose.position.x; - odom_tf.transform.translation.y = odom_msg.pose.pose.position.y; - odom_tf.transform.translation.z = odom_msg.pose.pose.position.z; - odom_tf.transform.rotation = odom_msg.pose.pose.orientation; - tf_broadcaster_->sendTransform(odom_tf); -} - -airsim_interfaces::msg::GPSYaw AirsimROSWrapper::get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const -{ - airsim_interfaces::msg::GPSYaw gps_msg; - gps_msg.latitude = geo_point.latitude; - gps_msg.longitude = geo_point.longitude; - gps_msg.altitude = geo_point.altitude; - return gps_msg; -} - -sensor_msgs::msg::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const -{ - sensor_msgs::msg::NavSatFix gps_msg; - gps_msg.latitude = geo_point.latitude; - gps_msg.longitude = geo_point.longitude; - gps_msg.altitude = geo_point.altitude; - return gps_msg; -} - -msr::airlib::GeoPoint AirsimROSWrapper::get_origin_geo_point() const -{ - HomeGeoPoint geo_point = AirSimSettings::singleton().origin_geopoint; - return geo_point.home_geo_point; -} - -VelCmd AirsimROSWrapper::get_airlib_world_vel_cmd(const airsim_interfaces::msg::VelCmd& msg) const -{ - VelCmd vel_cmd; - vel_cmd.x = msg.twist.linear.x; - vel_cmd.y = msg.twist.linear.y; - vel_cmd.z = msg.twist.linear.z; - vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - vel_cmd.yaw_mode.is_rate = true; - vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - return vel_cmd; -} - -VelCmd AirsimROSWrapper::get_airlib_body_vel_cmd(const airsim_interfaces::msg::VelCmd& msg, const msr::airlib::Quaternionr& airlib_quat) const -{ - VelCmd vel_cmd; - double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(airlib_quat)).getRPY(roll, pitch, yaw); // ros uses xyzw - - // todo do actual body frame? - vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame - vel_cmd.z = msg.twist.linear.z; - vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - vel_cmd.yaw_mode.is_rate = true; - // airsim uses degrees - vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - - return vel_cmd; -} - -geometry_msgs::msg::Transform AirsimROSWrapper::get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::AirSimSettings::Rotation& rotation) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = position.x(); - transform.translation.y = position.y(); - transform.translation.z = position.z(); - tf2::Quaternion quat; - quat.setRPY(rotation.roll, rotation.pitch, rotation.yaw); - transform.rotation.x = quat.x(); - transform.rotation.y = quat.y(); - transform.rotation.z = quat.z(); - transform.rotation.w = quat.w(); - - return transform; -} - -geometry_msgs::msg::Transform AirsimROSWrapper::get_transform_msg_from_airsim(const msr::airlib::Vector3r& position, const msr::airlib::Quaternionr& quaternion) -{ - geometry_msgs::msg::Transform transform; - transform.translation.x = position.x(); - transform.translation.y = position.y(); - transform.translation.z = position.z(); - transform.rotation.x = quaternion.x(); - transform.rotation.y = quaternion.y(); - transform.rotation.z = quaternion.z(); - transform.rotation.w = quaternion.w(); - - return transform; -} - -rclcpp::Time AirsimROSWrapper::chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const -{ - - auto dur = std::chrono::duration_cast(stamp.time_since_epoch()); - rclcpp::Time cur_time(dur.count()); - return cur_time; -} - -rclcpp::Time AirsimROSWrapper::airsim_timestamp_to_ros(const msr::airlib::TTimePoint& stamp) const -{ - // airsim appears to use chrono::system_clock with nanosecond precision - std::chrono::nanoseconds dur(stamp); - std::chrono::time_point tp(dur); - rclcpp::Time cur_time = chrono_timestamp_to_ros(tp); - return cur_time; -} - -void AirsimROSWrapper::drone_state_timer_cb() -{ - try { - // todo this is global origin - origin_geo_point_pub_->publish(origin_geo_point_msg_); - - // get the basic vehicle pose and environmental state - const auto now = update_state(); - - // on init, will publish 0 to /clock as expected for use_sim_time compatibility - if (!airsim_client_->simIsPaused()) { - // airsim_client needs to provide the simulation time in a future version of the API - ros_clock_.clock = now; - } - // publish the simulation clock - if (publish_clock_) { - clock_pub_->publish(ros_clock_); - } - - // publish vehicle state, odom, and all basic sensor types - publish_vehicle_state(); - - // send any commands out to the vehicles - update_commands(); - } - catch (rpc::rpc_error& e) { - std::string msg = e.get_error().as(); - RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API:\n%s", msg); - } -} - -void AirsimROSWrapper::update_and_publish_static_transforms(VehicleROS* vehicle_ros) -{ - if (vehicle_ros && !vehicle_ros->static_tf_msg_vec_.empty()) { - for (auto& static_tf_msg : vehicle_ros->static_tf_msg_vec_) { - static_tf_msg.header.stamp = vehicle_ros->stamp_; - static_tf_pub_->sendTransform(static_tf_msg); - } - } -} - -rclcpp::Time AirsimROSWrapper::update_state() -{ - bool got_sim_time = false; - rclcpp::Time curr_ros_time = nh_->now(); - - //should be easier way to get the sim time through API, something like: - //msr::airlib::Environment::State env = airsim_client_->simGetGroundTruthEnvironment(""); - //curr_ros_time = airsim_timestamp_to_ros(env.clock().nowNanos()); - - // iterate over drones - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - rclcpp::Time vehicle_time; - // get drone state from airsim - auto& vehicle_ros = vehicle_name_ptr_pair.second; - - // vehicle environment, we can get ambient temperature here and other truths - auto env_data = airsim_client_->simGetGroundTruthEnvironment(vehicle_ros->vehicle_name_); - - if (airsim_mode_ == AIRSIM_MODE::DRONE) { - auto drone = static_cast(vehicle_ros.get()); - auto rpc = static_cast(airsim_client_.get()); - drone->curr_drone_state_ = rpc->getMultirotorState(vehicle_ros->vehicle_name_); - - vehicle_time = airsim_timestamp_to_ros(drone->curr_drone_state_.timestamp); - if (!got_sim_time) { - curr_ros_time = vehicle_time; - got_sim_time = true; - } - - vehicle_ros->gps_sensor_msg_ = get_gps_sensor_msg_from_airsim_geo_point(drone->curr_drone_state_.gps_location); - vehicle_ros->gps_sensor_msg_.header.stamp = vehicle_time; - - vehicle_ros->curr_odom_ = get_odom_msg_from_multirotor_state(drone->curr_drone_state_); - } - else { - auto car = static_cast(vehicle_ros.get()); - auto rpc = static_cast(airsim_client_.get()); - car->curr_car_state_ = rpc->getCarState(vehicle_ros->vehicle_name_); - - vehicle_time = airsim_timestamp_to_ros(car->curr_car_state_.timestamp); - if (!got_sim_time) { - curr_ros_time = vehicle_time; - got_sim_time = true; - } - - vehicle_ros->gps_sensor_msg_ = get_gps_sensor_msg_from_airsim_geo_point(env_data.geo_point); - vehicle_ros->gps_sensor_msg_.header.stamp = vehicle_time; - - vehicle_ros->curr_odom_ = get_odom_msg_from_car_state(car->curr_car_state_); - - airsim_interfaces::msg::CarState state_msg = get_roscarstate_msg_from_car_state(car->curr_car_state_); - state_msg.header.frame_id = vehicle_ros->vehicle_name_; - car->car_state_msg_ = state_msg; - } - - vehicle_ros->stamp_ = vehicle_time; - - airsim_interfaces::msg::Environment env_msg = get_environment_msg_from_airsim(env_data); - env_msg.header.frame_id = vehicle_ros->vehicle_name_; - env_msg.header.stamp = vehicle_time; - vehicle_ros->env_msg_ = env_msg; - - // convert airsim drone state to ROS msgs - vehicle_ros->curr_odom_.header.frame_id = vehicle_ros->vehicle_name_; - vehicle_ros->curr_odom_.child_frame_id = vehicle_ros->odom_frame_id_; - vehicle_ros->curr_odom_.header.stamp = vehicle_time; - } - - return curr_ros_time; -} - -void AirsimROSWrapper::publish_vehicle_state() -{ - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - auto& vehicle_ros = vehicle_name_ptr_pair.second; - - // simulation environment truth - vehicle_ros->env_pub_->publish(vehicle_ros->env_msg_); - - if (airsim_mode_ == AIRSIM_MODE::CAR) { - // dashboard reading from car, RPM, gear, etc - auto car = static_cast(vehicle_ros.get()); - car->car_state_pub_->publish(car->car_state_msg_); - } - - // odom and transforms - vehicle_ros->odom_local_pub_->publish(vehicle_ros->curr_odom_); - publish_odom_tf(vehicle_ros->curr_odom_); - - // ground truth GPS position from sim/HITL - vehicle_ros->global_gps_pub_->publish(vehicle_ros->gps_sensor_msg_); - - for (auto& sensor_publisher : vehicle_ros->barometer_pubs_) { - auto baro_data = airsim_client_->getBarometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); - airsim_interfaces::msg::Altimeter alt_msg = get_altimeter_msg_from_airsim(baro_data); - alt_msg.header.frame_id = vehicle_ros->vehicle_name_; - sensor_publisher.publisher->publish(alt_msg); - } - - for (auto& sensor_publisher : vehicle_ros->imu_pubs_) { - auto imu_data = airsim_client_->getImuData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); - sensor_msgs::msg::Imu imu_msg = get_imu_msg_from_airsim(imu_data); - imu_msg.header.frame_id = vehicle_ros->vehicle_name_; - sensor_publisher.publisher->publish(imu_msg); - } - for (auto& sensor_publisher : vehicle_ros->distance_pubs_) { - auto distance_data = airsim_client_->getDistanceSensorData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); - sensor_msgs::msg::Range dist_msg = get_range_from_airsim(distance_data); - dist_msg.header.frame_id = vehicle_ros->vehicle_name_; - sensor_publisher.publisher->publish(dist_msg); - } - for (auto& sensor_publisher : vehicle_ros->gps_pubs_) { - auto gps_data = airsim_client_->getGpsData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); - sensor_msgs::msg::NavSatFix gps_msg = get_gps_msg_from_airsim(gps_data); - gps_msg.header.frame_id = vehicle_ros->vehicle_name_; - sensor_publisher.publisher->publish(gps_msg); - } - for (auto& sensor_publisher : vehicle_ros->magnetometer_pubs_) { - auto mag_data = airsim_client_->getMagnetometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name_); - sensor_msgs::msg::MagneticField mag_msg = get_mag_msg_from_airsim(mag_data); - mag_msg.header.frame_id = vehicle_ros->vehicle_name_; - sensor_publisher.publisher->publish(mag_msg); - } - - update_and_publish_static_transforms(vehicle_ros.get()); - } -} - -void AirsimROSWrapper::update_commands() -{ - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - auto& vehicle_ros = vehicle_name_ptr_pair.second; - - if (airsim_mode_ == AIRSIM_MODE::DRONE) { - auto drone = static_cast(vehicle_ros.get()); - - // send control commands from the last callback to airsim - if (drone->has_vel_cmd_) { - std::lock_guard guard(control_mutex_); - static_cast(airsim_client_.get())->moveByVelocityAsync(drone->vel_cmd_.x, drone->vel_cmd_.y, drone->vel_cmd_.z, vel_cmd_duration_, msr::airlib::DrivetrainType::MaxDegreeOfFreedom, drone->vel_cmd_.yaw_mode, drone->vehicle_name_); - } - drone->has_vel_cmd_ = false; - } - else { - // send control commands from the last callback to airsim - auto car = static_cast(vehicle_ros.get()); - if (car->has_car_cmd_) { - std::lock_guard guard(control_mutex_); - static_cast(airsim_client_.get())->setCarControls(car->car_cmd_, vehicle_ros->vehicle_name_); - } - car->has_car_cmd_ = false; - } - } - - // Only camera rotation, no translation movement of camera - if (has_gimbal_cmd_) { - std::lock_guard guard(control_mutex_); - airsim_client_->simSetCameraPose(gimbal_cmd_.camera_name, get_airlib_pose(0, 0, 0, gimbal_cmd_.target_quat), gimbal_cmd_.vehicle_name); - } - - has_gimbal_cmd_ = false; -} - -// airsim uses nans for zeros in settings.json. we set them to zeros here for handling tfs in ROS -void AirsimROSWrapper::set_nans_to_zeros_in_pose(VehicleSetting& vehicle_setting) const -{ - if (std::isnan(vehicle_setting.position.x())) - vehicle_setting.position.x() = 0.0; - - if (std::isnan(vehicle_setting.position.y())) - vehicle_setting.position.y() = 0.0; - - if (std::isnan(vehicle_setting.position.z())) - vehicle_setting.position.z() = 0.0; - - if (std::isnan(vehicle_setting.rotation.yaw)) - vehicle_setting.rotation.yaw = 0.0; - - if (std::isnan(vehicle_setting.rotation.pitch)) - vehicle_setting.rotation.pitch = 0.0; - - if (std::isnan(vehicle_setting.rotation.roll)) - vehicle_setting.rotation.roll = 0.0; -} - -// if any nan's in camera pose, set them to match vehicle pose (which has already converted any potential nans to zeros) -void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_setting, CameraSetting& camera_setting) const -{ - if (std::isnan(camera_setting.position.x())) - camera_setting.position.x() = vehicle_setting.position.x(); - - if (std::isnan(camera_setting.position.y())) - camera_setting.position.y() = vehicle_setting.position.y(); - - if (std::isnan(camera_setting.position.z())) - camera_setting.position.z() = vehicle_setting.position.z(); - - if (std::isnan(camera_setting.rotation.yaw)) - camera_setting.rotation.yaw = vehicle_setting.rotation.yaw; - - if (std::isnan(camera_setting.rotation.pitch)) - camera_setting.rotation.pitch = vehicle_setting.rotation.pitch; - - if (std::isnan(camera_setting.rotation.roll)) - camera_setting.rotation.roll = vehicle_setting.rotation.roll; -} - -void AirsimROSWrapper::convert_tf_msg_to_enu(geometry_msgs::msg::TransformStamped& tf_msg) -{ - std::swap(tf_msg.transform.translation.x, tf_msg.transform.translation.y); - std::swap(tf_msg.transform.rotation.x, tf_msg.transform.rotation.y); - tf_msg.transform.translation.z = -tf_msg.transform.translation.z; - tf_msg.transform.rotation.z = -tf_msg.transform.rotation.z; -} - -void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting) -{ - geometry_msgs::msg::TransformStamped vehicle_tf_msg; - vehicle_tf_msg.header.frame_id = world_frame_id_; - vehicle_tf_msg.header.stamp = nh_->now(); - vehicle_tf_msg.child_frame_id = vehicle_ros->vehicle_name_; - vehicle_tf_msg.transform = get_transform_msg_from_airsim(vehicle_setting.position, vehicle_setting.rotation); - - if (isENU_) { - convert_tf_msg_to_enu(vehicle_tf_msg); - } - - vehicle_ros->static_tf_msg_vec_.emplace_back(vehicle_tf_msg); -} - -void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const msr::airlib::LidarSimpleParams& lidar_setting) -{ - geometry_msgs::msg::TransformStamped lidar_tf_msg; - lidar_tf_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; - lidar_tf_msg.child_frame_id = vehicle_ros->vehicle_name_ + "/" + lidar_name; - lidar_tf_msg.transform = get_transform_msg_from_airsim(lidar_setting.relative_pose.position, lidar_setting.relative_pose.orientation); - - if (isENU_) { - convert_tf_msg_to_enu(lidar_tf_msg); - } - - vehicle_ros->static_tf_msg_vec_.emplace_back(lidar_tf_msg); -} - -void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting) -{ - geometry_msgs::msg::TransformStamped static_cam_tf_body_msg; - static_cam_tf_body_msg.header.frame_id = vehicle_ros->vehicle_name_ + "/" + odom_frame_id_; - static_cam_tf_body_msg.child_frame_id = camera_name + "_body/static"; - static_cam_tf_body_msg.transform = get_transform_msg_from_airsim(camera_setting.position, camera_setting.rotation); - - if (isENU_) { - convert_tf_msg_to_enu(static_cam_tf_body_msg); - } - - geometry_msgs::msg::TransformStamped static_cam_tf_optical_msg = static_cam_tf_body_msg; - static_cam_tf_optical_msg.child_frame_id = camera_name + "_optical/static"; - - tf2::Quaternion quat_cam_body; - tf2::Quaternion quat_cam_optical; - tf2::convert(static_cam_tf_body_msg.transform.rotation, quat_cam_body); - tf2::Matrix3x3 mat_cam_body(quat_cam_body); - tf2::Matrix3x3 mat_cam_optical; - mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); - mat_cam_optical.getRotation(quat_cam_optical); - quat_cam_optical.normalize(); - tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation); - - vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_body_msg); - vehicle_ros->static_tf_msg_vec_.emplace_back(static_cam_tf_optical_msg); -} - -void AirsimROSWrapper::img_response_timer_cb() -{ - try { - int image_response_idx = 0; - for (const auto& airsim_img_request_vehicle_name_pair : airsim_img_request_vehicle_name_pair_vec_) { - const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); - - if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) { - process_and_publish_img_response(img_response, image_response_idx, airsim_img_request_vehicle_name_pair.second); - image_response_idx += img_response.size(); - } - } - } - - catch (rpc::rpc_error& e) { - std::string msg = e.get_error().as(); - RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get image response.\n%s", msg); - } -} - -void AirsimROSWrapper::lidar_timer_cb() -{ - try { - for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - if (!vehicle_name_ptr_pair.second->lidar_pubs_.empty()) { - for (auto& lidar_publisher : vehicle_name_ptr_pair.second->lidar_pubs_) { - auto lidar_data = airsim_client_lidar_.getLidarData(lidar_publisher.sensor_name, vehicle_name_ptr_pair.first); - sensor_msgs::msg::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data, vehicle_name_ptr_pair.first); - lidar_publisher.publisher->publish(lidar_msg); - } - } - } - } - catch (rpc::rpc_error& e) { - std::string msg = e.get_error().as(); - RCLCPP_ERROR(nh_->get_logger(), "Exception raised by the API, didn't get image response.\n%s", msg); - } -} - -cv::Mat AirsimROSWrapper::manual_decode_depth(const ImageResponse& img_response) const -{ - cv::Mat mat(img_response.height, img_response.width, CV_32FC1, cv::Scalar(0)); - int img_width = img_response.width; - - for (int row = 0; row < img_response.height; row++) - for (int col = 0; col < img_width; col++) - mat.at(row, col) = img_response.image_data_float[row * img_width + col]; - return mat; -} - -std::shared_ptr AirsimROSWrapper::get_img_msg_from_response(const ImageResponse& img_response, - const rclcpp::Time curr_ros_time, - const std::string frame_id) -{ - unused(curr_ros_time); - std::shared_ptr img_msg_ptr = std::make_shared(); - img_msg_ptr->data = img_response.image_data_uint8; - img_msg_ptr->step = img_response.image_data_uint8.size() / img_response.height; - img_msg_ptr->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); - img_msg_ptr->header.frame_id = frame_id; - img_msg_ptr->height = img_response.height; - img_msg_ptr->width = img_response.width; - img_msg_ptr->encoding = "bgr8"; - if (is_vulkan_) - img_msg_ptr->encoding = "rgb8"; - img_msg_ptr->is_bigendian = 0; - return img_msg_ptr; -} - -std::shared_ptr AirsimROSWrapper::get_depth_img_msg_from_response(const ImageResponse& img_response, - const rclcpp::Time curr_ros_time, - const std::string frame_id) -{ - unused(curr_ros_time); - // todo using img_response.image_data_float direclty as done get_img_msg_from_response() throws an error, - // hence the dependency on opencv and cv_bridge. however, this is an extremely fast op, so no big deal. - cv::Mat depth_img = manual_decode_depth(img_response); - std::shared_ptr depth_img_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "32FC1", depth_img).toImageMsg(); - depth_img_msg->header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); - depth_img_msg->header.frame_id = frame_id; - return depth_img_msg; -} - -// todo have a special stereo pair mode and get projection matrix by calculating offset wrt drone body frame? -sensor_msgs::msg::CameraInfo AirsimROSWrapper::generate_cam_info(const std::string& camera_name, - const CameraSetting& camera_setting, - const CaptureSetting& capture_setting) const -{ - unused(camera_setting); - sensor_msgs::msg::CameraInfo cam_info_msg; - cam_info_msg.header.frame_id = camera_name + "_optical"; - cam_info_msg.height = capture_setting.height; - cam_info_msg.width = capture_setting.width; - float f_x = (capture_setting.width / 2.0) / tan(math_common::deg2rad(capture_setting.fov_degrees / 2.0)); - // todo focal length in Y direction should be same as X it seems. this can change in future a scene capture component which exactly correponds to a cine camera - // float f_y = (capture_setting.height / 2.0) / tan(math_common::deg2rad(fov_degrees / 2.0)); - cam_info_msg.k = { f_x, 0.0, capture_setting.width / 2.0, 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 1.0 }; - cam_info_msg.p = { f_x, 0.0, capture_setting.width / 2.0, 0.0, 0.0, f_x, capture_setting.height / 2.0, 0.0, 0.0, 0.0, 1.0, 0.0 }; - return cam_info_msg; -} - -void AirsimROSWrapper::process_and_publish_img_response(const std::vector& img_response_vec, const int img_response_idx, const std::string& vehicle_name) -{ - // todo add option to use airsim time (image_response.TTimePoint) like Gazebo /use_sim_time param - rclcpp::Time curr_ros_time = nh_->now(); - int img_response_idx_internal = img_response_idx; - - for (const auto& curr_img_response : img_response_vec) { - // todo publishing a tf for each capture type seems stupid. but it foolproofs us against render thread's async stuff, I hope. - // Ideally, we should loop over cameras and then captures, and publish only one tf. - publish_camera_tf(curr_img_response, curr_ros_time, vehicle_name, curr_img_response.camera_name); - - // todo simGetCameraInfo is wrong + also it's only for image type -1. - // msr::airlib::CameraInfo camera_info = airsim_client_.simGetCameraInfo(curr_img_response.camera_name); - - // update timestamp of saved cam info msgs - - camera_info_msg_vec_[img_response_idx_internal].header.stamp = airsim_timestamp_to_ros(curr_img_response.time_stamp); - cam_info_pub_vec_[img_response_idx_internal]->publish(camera_info_msg_vec_[img_response_idx_internal]); - - // DepthPlanar / DepthPerspective / DepthVis / DisparityNormalized - if (curr_img_response.pixels_as_float) { - image_pub_vec_[img_response_idx_internal].publish(get_depth_img_msg_from_response(curr_img_response, - curr_ros_time, - curr_img_response.camera_name + "_optical")); - } - // Scene / Segmentation / SurfaceNormals / Infrared - else { - image_pub_vec_[img_response_idx_internal].publish(get_img_msg_from_response(curr_img_response, - curr_ros_time, - curr_img_response.camera_name + "_optical")); - } - img_response_idx_internal++; - } -} - -// publish camera transforms -// camera poses are obtained from airsim's client API which are in (local) NED frame. -// We first do a change of basis to camera optical frame (Z forward, X right, Y down) -void AirsimROSWrapper::publish_camera_tf(const ImageResponse& img_response, const rclcpp::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id) -{ - unused(ros_time); - geometry_msgs::msg::TransformStamped cam_tf_body_msg; - cam_tf_body_msg.header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); - cam_tf_body_msg.header.frame_id = frame_id; - cam_tf_body_msg.child_frame_id = child_frame_id + "_body"; - cam_tf_body_msg.transform = get_transform_msg_from_airsim(img_response.camera_position, img_response.camera_orientation); - - if (isENU_) { - convert_tf_msg_to_enu(cam_tf_body_msg); - } - - geometry_msgs::msg::TransformStamped cam_tf_optical_msg; - cam_tf_optical_msg.header.stamp = airsim_timestamp_to_ros(img_response.time_stamp); - cam_tf_optical_msg.header.frame_id = frame_id; - cam_tf_optical_msg.child_frame_id = child_frame_id + "_optical"; - cam_tf_optical_msg.transform.translation = cam_tf_body_msg.transform.translation; - - tf2::Quaternion quat_cam_body; - tf2::Quaternion quat_cam_optical; - tf2::convert(cam_tf_body_msg.transform.rotation, quat_cam_body); - tf2::Matrix3x3 mat_cam_body(quat_cam_body); - // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body * matrix_cam_body_to_optical_inverse_; - // tf2::Matrix3x3 mat_cam_optical = matrix_cam_body_to_optical_ * mat_cam_body; - tf2::Matrix3x3 mat_cam_optical; - mat_cam_optical.setValue(mat_cam_body.getColumn(1).getX(), mat_cam_body.getColumn(2).getX(), mat_cam_body.getColumn(0).getX(), mat_cam_body.getColumn(1).getY(), mat_cam_body.getColumn(2).getY(), mat_cam_body.getColumn(0).getY(), mat_cam_body.getColumn(1).getZ(), mat_cam_body.getColumn(2).getZ(), mat_cam_body.getColumn(0).getZ()); - mat_cam_optical.getRotation(quat_cam_optical); - quat_cam_optical.normalize(); - tf2::convert(quat_cam_optical, cam_tf_optical_msg.transform.rotation); - - tf_broadcaster_->sendTransform(cam_tf_body_msg); - tf_broadcaster_->sendTransform(cam_tf_optical_msg); -} - -void AirsimROSWrapper::convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const -{ - int rows, cols; - rows = node["rows"].as(); - cols = node["cols"].as(); - const YAML::Node& data = node["data"]; - for (int i = 0; i < rows * cols; ++i) { - m.data[i] = data[i].as(); - } -} - -void AirsimROSWrapper::read_params_from_yaml_and_fill_cam_info_msg(const std::string& file_name, sensor_msgs::msg::CameraInfo& cam_info) const -{ - std::ifstream fin(file_name.c_str()); - YAML::Node doc = YAML::Load(fin); - - cam_info.width = doc[WIDTH_YML_NAME].as(); - cam_info.height = doc[HEIGHT_YML_NAME].as(); - - SimpleMatrix K_(3, 3, &cam_info.k[0]); - convert_yaml_to_simple_mat(doc[K_YML_NAME], K_); - SimpleMatrix R_(3, 3, &cam_info.r[0]); - convert_yaml_to_simple_mat(doc[R_YML_NAME], R_); - SimpleMatrix P_(3, 4, &cam_info.p[0]); - convert_yaml_to_simple_mat(doc[P_YML_NAME], P_); - - cam_info.distortion_model = doc[DMODEL_YML_NAME].as(); - - const YAML::Node& D_node = doc[D_YML_NAME]; - int D_rows, D_cols; - D_rows = D_node["rows"].as(); - D_cols = D_node["cols"].as(); - const YAML::Node& D_data = D_node["data"]; - cam_info.d.resize(D_rows * D_cols); - for (int i = 0; i < D_rows * D_cols; ++i) { - cam_info.d[i] = D_data[i].as(); - } -} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp b/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp deleted file mode 100755 index f907e0c9e1..0000000000 --- a/ros2/src/airsim_ros_pkgs/src/airsim_settings_parser.cpp +++ /dev/null @@ -1,43 +0,0 @@ -#include "airsim_settings_parser.h" - -AirSimSettingsParser::AirSimSettingsParser(const std::string& host_ip) - : host_ip_(host_ip) -{ - success_ = initializeSettings(); -} - -bool AirSimSettingsParser::success() -{ - return success_; -} - -bool AirSimSettingsParser::getSettingsText(std::string& settings_text) const -{ - msr::airlib::RpcLibClientBase airsim_client(host_ip_); - airsim_client.confirmConnection(); - - settings_text = airsim_client.getSettingsString(); - - return !settings_text.empty(); -} - -std::string AirSimSettingsParser::getSimMode() -{ - Settings& settings_json = Settings::loadJSonString(settings_text_); - return settings_json.getString("SimMode", ""); -} - -// mimics void ASimHUD::initializeSettings() -bool AirSimSettingsParser::initializeSettings() -{ - if (getSettingsText(settings_text_)) { - AirSimSettings::initializeSettings(settings_text_); - - AirSimSettings::singleton().load(std::bind(&AirSimSettingsParser::getSimMode, this)); - std::cout << "SimMode: " << AirSimSettings::singleton().simmode_name << std::endl; - - return true; - } - - return false; -} \ No newline at end of file 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 deleted file mode 100644 index 51fa4d565c..0000000000 --- a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple.cpp +++ /dev/null @@ -1,352 +0,0 @@ -#include "pd_position_controller_simple.h" -using namespace std::placeholders; - -bool PIDParams::load_from_rosparams(const shared_ptr nh) -{ - bool found = true; - - found = found && nh->get_parameter("kp_x", kp_x); - found = found && nh->get_parameter("kp_y", kp_y); - found = found && nh->get_parameter("kp_z", kp_z); - found = found && nh->get_parameter("kp_yaw", kp_yaw); - - found = found && nh->get_parameter("kd_x", kd_x); - found = found && nh->get_parameter("kd_y", kd_y); - found = found && nh->get_parameter("kd_z", kd_z); - found = found && nh->get_parameter("kd_yaw", kd_yaw); - - found = found && nh->get_parameter("reached_thresh_xyz", reached_thresh_xyz); - found = found && nh->get_parameter("reached_yaw_degrees", reached_yaw_degrees); - - return found; -} - -bool DynamicConstraints::load_from_rosparams(const std::shared_ptr nh) -{ - bool found = true; - - found = found && nh->get_parameter("max_vel_horz_abs", max_vel_horz_abs); - found = found && nh->get_parameter("max_vel_vert_abs", max_vel_vert_abs); - found = found && nh->get_parameter("max_yaw_rate_degree", max_yaw_rate_degree); - - return found; -} - -PIDPositionController::PIDPositionController(const std::shared_ptr nh) - : use_eth_lib_for_geodetic_conv_(true), nh_(nh), has_home_geo_(false), reached_goal_(false), has_goal_(false), has_odom_(false), got_goal_once_(false) -{ - params_.load_from_rosparams(nh_); - constraints_.load_from_rosparams(nh_); - initialize_ros(); - reset_errors(); -} - -void PIDPositionController::reset_errors() -{ - prev_error_.x = 0.0; - prev_error_.y = 0.0; - prev_error_.z = 0.0; - prev_error_.yaw = 0.0; -} - -void PIDPositionController::initialize_ros() -{ - vel_cmd_ = airsim_interfaces::msg::VelCmd(); - // ROS params - double update_control_every_n_sec; - nh_->get_parameter("update_control_every_n_sec", update_control_every_n_sec); - - auto parameters_client = std::make_shared(nh_, "/airsim_node"); - while (!parameters_client->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(nh_->get_logger(), "Interrupted while waiting for the service. Exiting."); - rclcpp::shutdown(); - } - RCLCPP_INFO(nh_->get_logger(), "service not available, waiting again..."); - } - - std::string vehicle_name; - - while (vehicle_name == "") { - vehicle_name = parameters_client->get_parameter("vehicle_name", vehicle_name); - RCLCPP_INFO_STREAM(nh_->get_logger(), "Waiting vehicle name"); - } - - // ROS publishers - airsim_vel_cmd_world_frame_pub_ = nh_->create_publisher("/airsim_node/" + vehicle_name + "/vel_cmd_world_frame", 1); - - // ROS subscribers - airsim_odom_sub_ = nh_->create_subscription("/airsim_node/" + vehicle_name + "/odom_local_ned", 50, std::bind(&PIDPositionController::airsim_odom_cb, this, _1)); - home_geopoint_sub_ = nh_->create_subscription("/airsim_node/home_geo_point", 50, std::bind(&PIDPositionController::home_geopoint_cb, this, _1)); - // todo publish this under global nodehandle / "airsim node" and hide it from user - local_position_goal_srvr_ = nh_->create_service("/airsim_node/local_position_goal", std::bind(&PIDPositionController::local_position_goal_srv_cb, this, _1, _2)); - local_position_goal_override_srvr_ = nh_->create_service("/airsim_node/local_position_goal/override", std::bind(&PIDPositionController::local_position_goal_srv_override_cb, this, _1, _2)); - gps_goal_srvr_ = nh_->create_service("/airsim_node/gps_goal", std::bind(&PIDPositionController::gps_goal_srv_cb, this, _1, _2)); - gps_goal_override_srvr_ = nh_->create_service("/airsim_node/gps_goal/override", std::bind(&PIDPositionController::gps_goal_srv_override_cb, this, _1, _2)); - - // ROS timers - update_control_cmd_timer_ = nh_->create_wall_timer(std::chrono::duration(update_control_every_n_sec), std::bind(&PIDPositionController::update_control_cmd_timer_cb, this)); -} - -void PIDPositionController::airsim_odom_cb(const nav_msgs::msg::Odometry::SharedPtr odom_msg) -{ - has_odom_ = true; - curr_odom_ = *odom_msg; - curr_position_.x = odom_msg->pose.pose.position.x; - curr_position_.y = odom_msg->pose.pose.position.y; - curr_position_.z = odom_msg->pose.pose.position.z; - curr_position_.yaw = utils::get_yaw_from_quat_msg(odom_msg->pose.pose.orientation); -} - -// todo maintain internal representation as eigen vec? -// todo check if low velocity if within thresh? -// todo maintain separate errors for XY and Z -void PIDPositionController::check_reached_goal() -{ - double diff_xyz = sqrt((target_position_.x - curr_position_.x) * (target_position_.x - curr_position_.x) + (target_position_.y - curr_position_.y) * (target_position_.y - curr_position_.y) + (target_position_.z - curr_position_.z) * (target_position_.z - curr_position_.z)); - - double diff_yaw = math_common::angular_dist(target_position_.yaw, curr_position_.yaw); - - // todo save this in degrees somewhere to avoid repeated conversion - if (diff_xyz < params_.reached_thresh_xyz && diff_yaw < math_common::deg2rad(params_.reached_yaw_degrees)) - reached_goal_ = true; -} - -bool PIDPositionController::local_position_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - // this tells the update timer callback to not do active hovering - if (!got_goal_once_) - got_goal_once_ = true; - - if (has_goal_ && !reached_goal_) { - // todo maintain array of position goals - RCLCPP_ERROR_STREAM(nh_->get_logger(), "denying position goal request-> I am still following the previous goal"); - return false; - } - - if (!has_goal_) { - target_position_.x = request->x; - target_position_.y = request->y; - target_position_.z = request->z; - target_position_.yaw = request->yaw; - RCLCPP_INFO_STREAM(nh_->get_logger(), "got goal: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); - - // todo error checks - // todo fill response - has_goal_ = true; - reached_goal_ = false; - reset_errors(); // todo - return true; - } - - // Already have goal, and have reached it - RCLCPP_INFO_STREAM(nh_->get_logger(), "Already have goal and have reached it"); - return false; -} - -bool PIDPositionController::local_position_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response) -{ - unused(response); - // this tells the update timer callback to not do active hovering - if (!got_goal_once_) - got_goal_once_ = true; - - target_position_.x = request->x; - target_position_.y = request->y; - target_position_.z = request->z; - target_position_.yaw = request->yaw; - RCLCPP_INFO_STREAM(nh_->get_logger(), "got goal: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); - - // todo error checks - // todo fill response - has_goal_ = true; - reached_goal_ = false; - reset_errors(); // todo - return true; -} - -void PIDPositionController::home_geopoint_cb(const airsim_interfaces::msg::GPSYaw::SharedPtr gps_msg) -{ - if (has_home_geo_) - return; - gps_home_msg_ = *gps_msg; - has_home_geo_ = true; - RCLCPP_INFO_STREAM(nh_->get_logger(), "GPS reference initializing " << gps_msg->latitude << ", " << gps_msg->longitude << ", " << gps_msg->altitude); - geodetic_converter_.initialiseReference(gps_msg->latitude, gps_msg->longitude, gps_msg->altitude); -} - -// todo do relative altitude, or add an option for the same? -bool PIDPositionController::gps_goal_srv_cb(const std::shared_ptr request, std::shared_ptr response) -{ - if (!has_home_geo_) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "I don't have home GPS coord. Can't go to GPS goal!"); - response->success = false; - } - - // convert GPS goal to NED goal - - if (!has_goal_) { - msr::airlib::GeoPoint goal_gps_point(request->latitude, request->longitude, request->altitude); - msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); - if (use_eth_lib_for_geodetic_conv_) { - double initial_latitude, initial_longitude, initial_altitude; - geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); - double n, e, d; - geodetic_converter_.geodetic2Ned(request->latitude, request->longitude, request->altitude, &n, &e, &d); - // RCLCPP_INFO_STREAM("geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); - target_position_.x = n; - target_position_.y = e; - target_position_.z = d; - } - else // use airlib::GeodeticToNedFast - { - RCLCPP_INFO_STREAM(nh_->get_logger(), "home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" - << "todo"); - msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); - target_position_.x = ned_goal[0]; - target_position_.y = ned_goal[1]; - target_position_.z = ned_goal[2]; - } - - target_position_.yaw = request->yaw; // todo - RCLCPP_INFO_STREAM(nh_->get_logger(), "got GPS goal: lat=" << goal_gps_point.latitude << " long=" << goal_gps_point.longitude << " alt=" << goal_gps_point.altitude << " yaw=" << target_position_.yaw); - RCLCPP_INFO_STREAM(nh_->get_logger(), "converted NED goal is: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); - - // todo error checks - // todo fill response - has_goal_ = true; - reached_goal_ = false; - reset_errors(); // todo - return true; - } - - // Already have goal, this shouldn't happen - RCLCPP_INFO_STREAM(nh_->get_logger(), "Goal already received, ignoring!"); - return false; -} - -// todo do relative altitude, or add an option for the same? -bool PIDPositionController::gps_goal_srv_override_cb(const std::shared_ptr request, std::shared_ptr response) -{ - if (!has_home_geo_) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "I don't have home GPS coord. Can't go to GPS goal!"); - response->success = false; - } - - // convert GPS goal to NED goal - - msr::airlib::GeoPoint goal_gps_point(request->latitude, request->longitude, request->altitude); - msr::airlib::GeoPoint gps_home(gps_home_msg_.latitude, gps_home_msg_.longitude, gps_home_msg_.altitude); - if (use_eth_lib_for_geodetic_conv_) { - double initial_latitude, initial_longitude, initial_altitude; - geodetic_converter_.getReference(&initial_latitude, &initial_longitude, &initial_altitude); - double n, e, d; - geodetic_converter_.geodetic2Ned(request->latitude, request->longitude, request->altitude, &n, &e, &d); - // RCLCPP_INFO_STREAM("geodetic_converter_ GPS reference initialized correctly (lat long in radians) " << initial_latitude << ", "<< initial_longitude << ", " << initial_altitude); - target_position_.x = n; - target_position_.y = e; - target_position_.z = d; - } - else // use airlib::GeodeticToNedFast - { - RCLCPP_INFO_STREAM(nh_->get_logger(), "home geopoint: lat=" << gps_home.latitude << " long=" << gps_home.longitude << " alt=" << gps_home.altitude << " yaw=" - << "todo"); - msr::airlib::Vector3r ned_goal = msr::airlib::EarthUtils::GeodeticToNedFast(goal_gps_point, gps_home); - target_position_.x = ned_goal[0]; - target_position_.y = ned_goal[1]; - target_position_.z = ned_goal[2]; - } - - target_position_.yaw = request->yaw; // todo - RCLCPP_INFO_STREAM(nh_->get_logger(), "got GPS goal: lat=" << goal_gps_point.latitude << " long=" << goal_gps_point.longitude << " alt=" << goal_gps_point.altitude << " yaw=" << target_position_.yaw); - RCLCPP_INFO_STREAM(nh_->get_logger(), "converted NED goal is: x=" << target_position_.x << " y=" << target_position_.y << " z=" << target_position_.z << " yaw=" << target_position_.yaw); - - // todo error checks - // todo fill response - has_goal_ = true; - reached_goal_ = false; - reset_errors(); // todo - return true; -} - -void PIDPositionController::update_control_cmd_timer_cb() -{ - // todo check if odometry is too old!! - // if no odom, don't do anything. - if (!has_odom_) { - RCLCPP_ERROR_STREAM(nh_->get_logger(), "Waiting for odometry!"); - return; - } - - if (has_goal_) { - check_reached_goal(); - if (reached_goal_) { - RCLCPP_INFO_STREAM(nh_->get_logger(), "Reached goal! Hovering at position."); - has_goal_ = false; - // dear future self, this function doesn't return coz we need to keep on actively hovering at last goal pose. don't act smart - } - else { - RCLCPP_INFO_STREAM(nh_->get_logger(), "Moving to goal."); - } - } - - // 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_) { - compute_control_cmd(); - enforce_dynamic_constraints(); - publish_control_cmd(); - } -} - -void PIDPositionController::compute_control_cmd() -{ - curr_error_.x = target_position_.x - curr_position_.x; - curr_error_.y = target_position_.y - curr_position_.y; - curr_error_.z = target_position_.z - curr_position_.z; - curr_error_.yaw = math_common::angular_dist(curr_position_.yaw, target_position_.yaw); - - double p_term_x = params_.kp_x * curr_error_.x; - double p_term_y = params_.kp_y * curr_error_.y; - double p_term_z = params_.kp_z * curr_error_.z; - double p_term_yaw = params_.kp_yaw * curr_error_.yaw; - - double d_term_x = params_.kd_x * prev_error_.x; - double d_term_y = params_.kd_y * prev_error_.y; - double d_term_z = params_.kd_z * prev_error_.z; - double d_term_yaw = params_.kp_yaw * prev_error_.yaw; - - prev_error_ = curr_error_; - - vel_cmd_.twist.linear.x = p_term_x + d_term_x; - vel_cmd_.twist.linear.y = p_term_y + d_term_y; - vel_cmd_.twist.linear.z = p_term_z + d_term_z; - vel_cmd_.twist.angular.z = p_term_yaw + d_term_yaw; // todo -} - -void PIDPositionController::enforce_dynamic_constraints() -{ - double vel_norm_horz = sqrt((vel_cmd_.twist.linear.x * vel_cmd_.twist.linear.x) + (vel_cmd_.twist.linear.y * vel_cmd_.twist.linear.y)); - - if (vel_norm_horz > constraints_.max_vel_horz_abs) { - vel_cmd_.twist.linear.x = (vel_cmd_.twist.linear.x / vel_norm_horz) * constraints_.max_vel_horz_abs; - vel_cmd_.twist.linear.y = (vel_cmd_.twist.linear.y / vel_norm_horz) * constraints_.max_vel_horz_abs; - } - - if (std::fabs(vel_cmd_.twist.linear.z) > constraints_.max_vel_vert_abs) { - // todo just add a sgn funciton in common utils? return double to be safe. - // template double sgn(T val) { return (T(0) < val) - (val < T(0)); } - vel_cmd_.twist.linear.z = (vel_cmd_.twist.linear.z / std::fabs(vel_cmd_.twist.linear.z)) * constraints_.max_vel_vert_abs; - } - // todo yaw limits - if (std::fabs(vel_cmd_.twist.linear.z) > constraints_.max_yaw_rate_degree) { - // todo just add a sgn funciton in common utils? return double to be safe. - // template double sgn(T val) { return (T(0) < val) - (val < T(0)); } - vel_cmd_.twist.linear.z = (vel_cmd_.twist.linear.z / std::fabs(vel_cmd_.twist.linear.z)) * constraints_.max_yaw_rate_degree; - } -} - -void PIDPositionController::publish_control_cmd() -{ - airsim_vel_cmd_world_frame_pub_->publish(vel_cmd_); -} \ No newline at end of file diff --git a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp b/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp deleted file mode 100755 index 42cc25cf2a..0000000000 --- a/ros2/src/airsim_ros_pkgs/src/pd_position_controller_simple_node.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include -#include "pd_position_controller_simple.h" - -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("pid_position_controller_simple_node", node_options); - - PIDPositionController controller(nh); - - rclcpp::spin(nh); - return 0; -} \ No newline at end of file diff --git a/tools/Dockerfile-ROS2 b/tools/Dockerfile-ROS2 deleted file mode 100644 index 73f801043e..0000000000 --- a/tools/Dockerfile-ROS2 +++ /dev/null @@ -1,12 +0,0 @@ -FROM ros:foxy-ros-base - -RUN apt-get update &&\ - apt-get install -y\ - apt-utils \ - gcc-8 g++-8 \ - ros-$ROS_DISTRO-tf2-sensor-msgs ros-$ROS_DISTRO-tf2-geometry-msgs ros-$ROS_DISTRO-mavros* \ - ros-$ROS_DISTRO-vision-opencv ros-$ROS_DISTRO-image-transport \ - libyaml-cpp-dev &&\ - echo 'source /opt/ros/$ROS_DISTRO/setup.bash' >> ~/.bashrc &&\ - rm -rf /var/lib/apt/lists/* &&\ - apt-get clean \ No newline at end of file