diff --git a/.gitignore b/.gitignore index f1a03ac777..c1564db29d 100644 --- a/.gitignore +++ b/.gitignore @@ -307,7 +307,7 @@ cmake_build/ /boost /[Ee]igen /build_debug -/build_gcc_debug +/build_release /cmake/CPackSourceConfig.cmake /cmake/CPackConfig.cmake /cmake/arch.c diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp index d277abb774..260fc53e02 100644 --- a/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -675,7 +675,7 @@ class RpcLibAdapatorsBase { DistanceSensorData() {} - DistanceSensorData(const msr::airlib::DistanceBase::Output& s) + DistanceSensorData(const msr::airlib::DistanceSensorData& s) { time_stamp = s.time_stamp; distance = s.distance; @@ -684,9 +684,9 @@ class RpcLibAdapatorsBase { relative_pose = s.relative_pose; } - msr::airlib::DistanceBase::Output to() const + msr::airlib::DistanceSensorData to() const { - msr::airlib::DistanceBase::Output d; + msr::airlib::DistanceSensorData d; d.time_stamp = time_stamp; d.distance = distance; diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 77bb15aaf9..44cf351f62 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -50,7 +50,10 @@ class RpcLibClientBase { vector simListSceneObjects(const string& name_regex = string(".*")) const; Pose simGetObjectPose(const std::string& object_name) const; + bool simLoadLevel(const string& level_name); + 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); //task management APIs void cancelLastTask(const std::string& vehicle_name = ""); @@ -81,7 +84,7 @@ class RpcLibClientBase { msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name = "", const std::string& vehicle_name = "") const; msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = "") const; msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = "") const; - msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const; + msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = "") const; Pose simGetVehiclePose(const std::string& vehicle_name = "") const; void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = ""); @@ -95,7 +98,7 @@ class RpcLibClientBase { CollisionInfo simGetCollisionInfo(const std::string& vehicle_name = "") const; CameraInfo simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name = "") const; - void simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name = ""); + void simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name = ""); void simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name = ""); msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const; @@ -103,6 +106,11 @@ class RpcLibClientBase { std::vector simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0); + // Recording APIs + void startRecording(); + void stopRecording(); + bool isRecording(); + protected: void* getClient(); const void* getClient() const; diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index a307baeaf3..a5c222207a 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -160,7 +160,7 @@ class VehicleApiBase : public UpdatableObject { } // Distance Sensor API - virtual DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name) const + virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const { auto *distance_sensor = static_cast(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance)); if (distance_sensor == nullptr) diff --git a/AirLib/include/api/VehicleSimApiBase.hpp b/AirLib/include/api/VehicleSimApiBase.hpp index 25b9579b03..471a2cfd85 100644 --- a/AirLib/include/api/VehicleSimApiBase.hpp +++ b/AirLib/include/api/VehicleSimApiBase.hpp @@ -54,7 +54,7 @@ class VehicleSimApiBase : public msr::airlib::UpdatableObject { virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0; virtual CameraInfo getCameraInfo(const std::string& camera_name) const = 0; - virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) = 0; + virtual void setCameraPose(const std::string& camera_name, const Pose& pose) = 0; virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) = 0; virtual CollisionInfo getCollisionInfo() const = 0; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 035063e771..e07cc96363 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/AirLib/include/api/WorldSimApiBase.hpp @@ -25,6 +25,11 @@ class WorldSimApiBase { virtual ~WorldSimApiBase() = default; + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string& level_name) = 0; + virtual string spawnObject(string& object_name, const string& load_component, const Pose& pose, const Vector3r& scale) = 0; + virtual bool destroyObject(const string& object_name) = 0; + virtual bool isPaused() const = 0; virtual void reset() = 0; virtual void pause(bool is_paused) = 0; @@ -54,10 +59,17 @@ class WorldSimApiBase { virtual std::vector listSceneObjects(const std::string& name_regex) const = 0; virtual Pose getObjectPose(const std::string& object_name) const = 0; + virtual Vector3r getObjectScale(const std::string& object_name) const = 0; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 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 vector getMeshPositionVertexBuffers() const = 0; + + // Recording APIs + virtual void startRecording() = 0; + virtual void stopRecording() = 0; + virtual bool isRecording() const = 0; }; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index ce9fd75f5d..348f235c8d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -25,8 +25,8 @@ struct AirSimSettings { public: //types static constexpr int kSubwindowCount = 3; //must be >= 3 for now static constexpr char const * kVehicleTypePX4 = "px4multirotor"; - static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; - static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; + static constexpr char const * kVehicleTypeArduCopterSolo = "arducoptersolo"; + static constexpr char const * kVehicleTypeSimpleFlight = "simpleflight"; static constexpr char const * kVehicleTypeArduCopter = "arducopter"; static constexpr char const * kVehicleTypePhysXCar = "physxcar"; static constexpr char const * kVehicleTypeArduRover = "ardurover"; @@ -196,6 +196,12 @@ struct AirSimSettings { }; struct DistanceSetting : SensorSetting { + // shared defaults + real_T min_distance = 20.0f / 100; //m + real_T max_distance = 4000.0f / 100; //m + Vector3r position = VectorMath::nanVector(); + Rotation rotation = Rotation::nanRotation(); + bool draw_debug_points = false; }; struct LidarSetting : SensorSetting { @@ -294,9 +300,9 @@ struct AirSimSettings { std::map params; }; - struct MavLinkVehicleSetting : public VehicleSetting { - MavLinkConnectionInfo connection_info; - }; + struct MavLinkVehicleSetting : public VehicleSetting { + MavLinkConnectionInfo connection_info; + }; struct SegmentationSetting { enum class InitMethodType { @@ -327,6 +333,7 @@ struct AirSimSettings { public: //fields std::string simmode_name = ""; + std::string level_name = ""; std::vector subwindow_settings; RecordingSetting recording_setting; @@ -340,7 +347,7 @@ struct AirSimSettings { int initial_view_mode = 2; //ECameraDirectorMode::CAMERA_DIRECTOR_MODE_FLY_WITH_ME bool enable_rpc = true; std::string api_server_address = ""; - int api_port = RpcLibPort; + int api_port = RpcLibPort; std::string physics_engine_name = ""; std::string clock_type = ""; @@ -352,8 +359,8 @@ struct AirSimSettings { std::map> vehicles; CameraSetting camera_defaults; CameraDirectorSetting camera_director; - float speed_unit_factor = 1.0f; - std::string speed_unit_label = "m\\s"; + float speed_unit_factor = 1.0f; + std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; public: //methods @@ -379,6 +386,7 @@ struct AirSimSettings { checkSettingsVersion(settings_json); loadCoreSimModeSettings(settings_json, simmode_getter); + loadLevelSettings(settings_json); loadDefaultCameraSetting(settings_json, camera_defaults); loadCameraDirectorSetting(settings_json, camera_director, simmode_name); loadSubWindowsSettings(settings_json, subwindow_settings); @@ -514,6 +522,11 @@ struct AirSimSettings { physics_engine_name = "PhysX"; //this value is only informational for now } } + + void loadLevelSettings(const Settings& settings_json) + { + level_name = settings_json.getString("Default Environment", ""); + } void loadViewModeSettings(const Settings& settings_json) { @@ -626,7 +639,7 @@ struct AirSimSettings { { //these settings_json are expected in same section, not in another child std::unique_ptr vehicle_setting_p = std::unique_ptr(new MavLinkVehicleSetting()); - MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); + MavLinkVehicleSetting* vehicle_setting = static_cast(vehicle_setting_p.get()); //TODO: we should be selecting remote if available else keyboard //currently keyboard is not supported so use rc as default @@ -1021,7 +1034,7 @@ struct AirSimSettings { //because for docker container default is 0.0.0.0 and people get really confused why things //don't work api_server_address = settings_json.getString("LocalHostIp", ""); - api_port = settings_json.getInt("ApiServerPort", RpcLibPort); + api_port = settings_json.getInt("ApiServerPort", RpcLibPort); is_record_ui_visible = settings_json.getBool("RecordUIVisible", true); engine_sound = settings_json.getBool("EngineSound", false); enable_rpc = settings_json.getBool("EnableRpc", enable_rpc); @@ -1154,10 +1167,12 @@ struct AirSimSettings { static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json) { - unused(distance_setting); - unused(settings_json); + distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance); + distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance); + distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points); - //TODO: set from json as needed + distance_setting.position = createVectorSetting(settings_json, distance_setting.position); + distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation); } static void initializeLidarSetting(LidarSetting& lidar_setting, const Settings& settings_json) diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index 350aad30a0..835a76a84a 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -311,6 +311,17 @@ struct LidarData { {} }; +struct DistanceSensorData { + TTimePoint time_stamp; + real_T distance; //meters + real_T min_distance; //m + real_T max_distance; //m + Pose relative_pose; + + DistanceSensorData() + {} +}; + struct MeshPositionVertexBuffersResponse { Vector3r position; Quaternionr orientation; diff --git a/AirLib/include/common/VectorMath.hpp b/AirLib/include/common/VectorMath.hpp index 114a673416..732994eb8e 100644 --- a/AirLib/include/common/VectorMath.hpp +++ b/AirLib/include/common/VectorMath.hpp @@ -324,6 +324,9 @@ class VectorMathT { static Vector3T toAngularVelocity(const QuaternionT& start, const QuaternionT& end, RealT dt) { + if (dt == 0) + return Vector3T(0, 0, 0); + RealT p_s, r_s, y_s; toEulerianAngle(start, p_s, r_s, y_s); diff --git a/AirLib/include/sensors/SensorFactory.hpp b/AirLib/include/sensors/SensorFactory.hpp index 1fffe86411..ed69b4abb7 100644 --- a/AirLib/include/sensors/SensorFactory.hpp +++ b/AirLib/include/sensors/SensorFactory.hpp @@ -57,6 +57,8 @@ class SensorFactory { } } } + + virtual ~SensorFactory() = default; }; diff --git a/AirLib/include/sensors/distance/DistanceBase.hpp b/AirLib/include/sensors/distance/DistanceBase.hpp index a0365b88aa..0200873a7f 100644 --- a/AirLib/include/sensors/distance/DistanceBase.hpp +++ b/AirLib/include/sensors/distance/DistanceBase.hpp @@ -16,16 +16,6 @@ class DistanceBase : public SensorBase { : SensorBase(sensor_name) {} -public: //types - struct Output { //same fields as ROS message - TTimePoint time_stamp; - real_T distance; //meters - real_T min_distance;//m - real_T max_distance;//m - Pose relative_pose; - }; - - public: virtual void reportState(StateReporter& reporter) override { @@ -35,20 +25,20 @@ class DistanceBase : public SensorBase { reporter.writeValue("Dist-Curr", output_.distance); } - const Output& getOutput() const + const DistanceSensorData& getOutput() const { return output_; } protected: - void setOutput(const Output& output) + void setOutput(const DistanceSensorData& output) { output_ = output; } private: - Output output_; + DistanceSensorData output_; }; diff --git a/AirLib/include/sensors/distance/DistanceSimple.hpp b/AirLib/include/sensors/distance/DistanceSimple.hpp index 74bb8069d3..45adcdddc3 100644 --- a/AirLib/include/sensors/distance/DistanceSimple.hpp +++ b/AirLib/include/sensors/distance/DistanceSimple.hpp @@ -14,7 +14,7 @@ namespace msr { namespace airlib { -class DistanceSimple : public DistanceBase { +class DistanceSimple : public DistanceBase { public: DistanceSimple(const AirSimSettings::DistanceSetting& setting = AirSimSettings::DistanceSetting()) : DistanceBase(setting.sensor_name) @@ -62,17 +62,18 @@ class DistanceSimple : public DistanceBase { virtual ~DistanceSimple() = default; -protected: - virtual real_T getRayLength(const Pose& pose) = 0; - const DistanceSimpleParams& getParams() + const DistanceSimpleParams& getParams() const { return params_; } +protected: + virtual real_T getRayLength(const Pose& pose) = 0; + private: //methods - Output getOutputInternal() + DistanceSensorData getOutputInternal() { - Output output; + DistanceSensorData output; const GroundTruth& ground_truth = getGroundTruth(); //order of Pose addition is important here because it also adds quaternions which is not commutative! @@ -97,7 +98,7 @@ class DistanceSimple : public DistanceBase { RandomGeneratorGausianR uncorrelated_noise_; FrequencyLimiter freq_limiter_; - DelayLine delay_line_; + DelayLine delay_line_; //start time }; diff --git a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp index a028365f18..fa1c95b5de 100644 --- a/AirLib/include/sensors/distance/DistanceSimpleParams.hpp +++ b/AirLib/include/sensors/distance/DistanceSimpleParams.hpp @@ -14,7 +14,13 @@ namespace msr { namespace airlib { struct DistanceSimpleParams { real_T min_distance = 20.0f / 100; //m real_T max_distance = 4000.0f / 100; //m - Pose relative_pose; + + Pose relative_pose { + Vector3r(0,0,-1), // position - a little above vehicle (especially for cars) or Vector3r::Zero() + Quaternionr::Identity() // orientation - by default Quaternionr(1, 0, 0, 0) + }; + + bool draw_debug_points = false; /* Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters @@ -40,7 +46,33 @@ struct DistanceSimpleParams { void initializeFromSettings(const AirSimSettings::DistanceSetting& settings) { - unused(settings); + std::string simmode_name = AirSimSettings::singleton().simmode_name; + + min_distance = settings.min_distance; + max_distance = settings.max_distance; + + draw_debug_points = settings.draw_debug_points; + + relative_pose.position = settings.position; + if (std::isnan(relative_pose.position.x())) + relative_pose.position.x() = 0; + if (std::isnan(relative_pose.position.y())) + relative_pose.position.y() = 0; + if (std::isnan(relative_pose.position.z())) { + if (simmode_name == "Multirotor") + relative_pose.position.z() = 0; + else + relative_pose.position.z() = -1; // a little bit above for cars + } + + float pitch, roll, yaw; + pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0; + roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0; + yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0; + relative_pose.orientation = VectorMath::toQuaternion( + Utils::degreesToRadians(pitch), //pitch - rotation around Y axis + Utils::degreesToRadians(roll), //roll - rotation around X axis + Utils::degreesToRadians(yaw)); //yaw - rotation around Z axis } }; diff --git a/AirLib/include/vehicles/car/api/CarApiBase.hpp b/AirLib/include/vehicles/car/api/CarApiBase.hpp index 1b20728b48..3f012b51e7 100644 --- a/AirLib/include/vehicles/car/api/CarApiBase.hpp +++ b/AirLib/include/vehicles/car/api/CarApiBase.hpp @@ -68,6 +68,16 @@ class CarApiBase : public VehicleApiBase { kinematics_estimated(kinematics_estimated_val), timestamp(timestamp_val) { } + + //shortcuts + const Vector3r& getPosition() const + { + return kinematics_estimated.pose.position; + } + const Quaternionr& getOrientation() const + { + return kinematics_estimated.pose.orientation; + } }; public: @@ -151,4 +161,4 @@ class CarApiBase : public VehicleApiBase { }} //namespace -#endif \ No newline at end of file +#endif diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp index c9b0d84a41..37ce927c36 100644 --- a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp @@ -33,7 +33,7 @@ class ArduRoverApi : public CarApiBase { ArduRoverApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) : CarApiBase(vehicle_setting, sensor_factory, state, environment), - state_(state), home_geopoint_(home_geopoint) + home_geopoint_(home_geopoint) { connection_info_ = static_cast(vehicle_setting)->connection_info; sensors_ = &getSensors(); @@ -277,7 +277,6 @@ class ArduRoverApi : public CarApiBase { const SensorCollection* sensors_; CarControls last_controls_; - const Kinematics::State& state_; GeoPoint home_geopoint_; CarState last_car_state_; }; diff --git a/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp index 1b9362da26..8e2cbfe47f 100644 --- a/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp +++ b/AirLib/include/vehicles/car/firmwares/physxcar/PhysXCarApi.hpp @@ -13,7 +13,7 @@ class PhysXCarApi : public CarApiBase { PhysXCarApi(const AirSimSettings::VehicleSetting* vehicle_setting, std::shared_ptr sensor_factory, const Kinematics::State& state, const Environment& environment, const msr::airlib::GeoPoint& home_geopoint) : CarApiBase(vehicle_setting, sensor_factory, state, environment), - home_geopoint_(home_geopoint), state_(state) + home_geopoint_(home_geopoint) {} ~PhysXCarApi() @@ -88,7 +88,6 @@ class PhysXCarApi : public CarApiBase { bool api_control_enabled_ = false; GeoPoint home_geopoint_; CarControls last_controls_; - const Kinematics::State& state_; CarState last_car_state_; }; diff --git a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp index d7135d52d6..29566c9155 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorParams.hpp @@ -36,12 +36,12 @@ class MultiRotorParams { Vector3r body_box; /*********** optional parameters with defaults ***********/ - real_T linear_drag_coefficient = 1.3f / 4.0f; + real_T linear_drag_coefficient = 1.3f / 4.0f; //sample value 1.3 from http://klsin.bpmsg.com/how-fast-can-a-quadcopter-fly/, but divided by 4 to account // for nice streamlined frame design and allow higher top speed which is more fun. //angular coefficient is usually 10X smaller than linear, however we should replace this with exact number //http://physics.stackexchange.com/q/304742/14061 - real_T angular_drag_coefficient = linear_drag_coefficient; + real_T angular_drag_coefficient = linear_drag_coefficient; real_T restitution = 0.55f; // value of 1 would result in perfectly elastic collisions, 0 would be completely inelastic. real_T friction = 0.5f; RotorParams rotor_params; @@ -100,27 +100,27 @@ class MultiRotorParams { /// which shows that given an array of 4 motors, the first is placed top right and flies counter clockwise (CCW) and /// the second is placed bottom left, and also flies CCW. The third in the array is placed top left and flies clockwise (CW) /// while the last is placed bottom right and also flies clockwise. This is how the 4 items in the arm_lengths and - /// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right, - /// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1), - /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example, + /// arm_angles arrays will be used. So arm_lengths is 4 numbers (in meters) where four arm lengths, 0 is top right, + /// 1 is bottom left, 2 is top left and 3 is bottom right. arm_angles is 4 numbers (in degrees) relative to forward vector (0,1), + /// provided in the same order where 0 is top right, 1 is bottom left, 2 is top left and 3 is bottom right, so for example, /// the angles for a regular symmetric X pattern would be 45, 225, 315, 135. The rotor_z is the offset of each motor upwards - /// relative to the center of mass (in meters). + /// relative to the center of mass (in meters). static void initializeRotorQuadX(vector& rotor_poses /* the result we are building */, - uint rotor_count /* must be 4 */, - real_T arm_lengths[], + uint rotor_count /* must be 4 */, + real_T arm_lengths[], real_T rotor_z /* z relative to center of gravity */) { Vector3r unit_z(0, 0, -1); //NED frame if (rotor_count == 4) { rotor_poses.clear(); - /* Note: rotor_poses are built in this order: + /* Note: rotor_poses are built in this order: x-axis (2) | (0) | -------------- y-axis | - (1) | (3) + (1) | (3) */ // vectors below are rotated according to NED left hand rule (so the vectors are rotated counter clockwise). Quaternionr quadx_rot(AngleAxisr(M_PIf / 4, unit_z)); @@ -128,9 +128,9 @@ class MultiRotorParams { unit_z, RotorTurningDirection::RotorTurningDirectionCCW); rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(0, -arm_lengths[1], rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCCW); - rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true), + rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[2], 0, rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCW); - rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true), + rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(-arm_lengths[3], 0, rotor_z), quadx_rot, true), unit_z, RotorTurningDirection::RotorTurningDirectionCW); } else @@ -153,8 +153,8 @@ class MultiRotorParams { \ / \/ (1)-------(0) y-axis - /\ - / \ + /\ + / \ (5) (3) */ @@ -200,9 +200,9 @@ class MultiRotorParams { inertia = Matrix3x3r::Zero(); //http://farside.ph.utexas.edu/teaching/336k/Newtonhtml/node64.html - inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z()); - inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z()); - inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y()); + inertia(0, 0) = box_mass / 12.0f * (body_box.y()*body_box.y() + body_box.z()*body_box.z()); + inertia(1, 1) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.z()*body_box.z()); + inertia(2, 2) = box_mass / 12.0f * (body_box.x()*body_box.x() + body_box.y()*body_box.y()); for (size_t i = 0; i < rotor_poses.size(); ++i) { const auto& pos = rotor_poses.at(i).position; @@ -212,6 +212,194 @@ class MultiRotorParams { } } + // Some Frame types which can be used by different firmwares + // Specific frame configurations, modifications can be done in the Firmware Params + + void setupFrameGenericQuad(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.2275f); + + //set up mass + params.mass = 1.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust + // given new thrust coefficients, motor max_rpm and propeller diameter. + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameGenericHex(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 6; + std::vector arm_lengths(params.rotor_count, 0.2275f); + + //set up mass + params.mass = 1.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust + // given new thrust coefficients, motor max_rpm and propeller diameter. + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameFlamewheel(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.225f); + + //set up mass + params.mass = 1.635f; + real_T motor_assembly_weight = 0.052f; + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + params.rotor_params.C_T = 0.11f; + params.rotor_params.C_P = 0.047f; + params.rotor_params.max_rpm = 9500; + params.rotor_params.calculateMaxThrust(); + params.linear_drag_coefficient *= 4; // make top speed more real. + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; + real_T rotor_z = 0.15f; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameFlamewheelFLA(Params& params) + { + //set up arm lengths + //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution + params.rotor_count = 4; + std::vector arm_lengths(params.rotor_count, 0.225f); + + //set up mass + params.mass = 2.25f; + real_T motor_assembly_weight = 0.1f; + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + params.rotor_params.C_T = 0.2f; + params.rotor_params.C_P = 0.1f; + params.rotor_params.max_rpm = 9324; + params.rotor_params.calculateMaxThrust(); + params.linear_drag_coefficient *= 4; // make top speed more real. + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; + real_T rotor_z = 0.15f; + + //computer rotor poses + initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + + void setupFrameBlacksheep(Params& params) + { + /* + Motor placement: + x + (2) | (0) + | + ------------ y + | + (1) | (3) + | + + */ + //set up arm lengths + //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98) + params.rotor_count = 4; + std::vector arm_lengths; + + Vector3r unit_z(0, 0, -1); //NED frame + + // relative to Forward vector in the order (0,3,1,2) required by quad X pattern + // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg + arm_lengths.push_back(0.22f); + arm_lengths.push_back(0.255f); + arm_lengths.push_back(0.22f); + arm_lengths.push_back(0.255f); + + // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation + // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below: + // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation) + std::vector arm_angles; + arm_angles.push_back(-55.0f); + arm_angles.push_back(125.0f); + arm_angles.push_back(55.0f); + arm_angles.push_back(-125.0f); + + // quad X pattern + std::vector rotor_directions; + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); + rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); + + // data from + // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/ + //set up mass + params.mass = 2.0f; //can be varied from 0.800 to 1.600 + real_T motor_assembly_weight = 0.052f; // weight for TBS motors + real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; + + // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close: + // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png + params.rotor_params.C_T = 0.11f; + params.rotor_params.C_P = 0.047f; + params.rotor_params.max_rpm = 9500; + params.rotor_params.calculateMaxThrust(); + + //set up dimensions of core body box or abdomen (not including arms). + params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f; + real_T rotor_z = 2.5f / 100; + + //computer rotor poses + params.rotor_poses.clear(); + for (uint i = 0; i < 4; i++) + { + Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); + params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); + }; + + //compute inertia matrix + computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); + } + private: Params params_; SensorCollection sensors_; //maintains sensor type indexed collection of sensors diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp index 68ce32760a..400ac408ed 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp @@ -449,7 +449,7 @@ class ArduCopterApi : public MultirotorApiBase { recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100); } - for (auto i = 0; i < RotorControlCount && i < kArduCopterRotorControlCount; ++i) { + for (auto i = 0; i < kArduCopterRotorControlCount; ++i) { rotor_controls_[i] = pkt.pwm[i]; } @@ -476,10 +476,7 @@ class ArduCopterApi : public MultirotorApiBase { RCData last_rcData_; bool is_rc_connected_; - // TODO: Increase to 6 or 8 for hexa or larger frames, 11 was used in SoloAPI - static const int RotorControlCount = 4; - - float rotor_controls_[RotorControlCount]; + float rotor_controls_[kArduCopterRotorControlCount]; }; }} //namespace diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp index 116d2814b3..38237602d8 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterParams.hpp @@ -49,37 +49,6 @@ class ArduCopterParams : public MultiRotorParams { } private: - void setupFrameGenericQuad(Params& params) - { - /******* Below is same config as PX4 generic model ********/ - - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - - //leave everything else to defaults - } - AirSimSettings::MavLinkConnectionInfo connection_info_; std::shared_ptr sensor_factory_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp index 5bb8b6e470..d13185d94b 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp @@ -116,15 +116,13 @@ class MavLinkMultirotorApi : public MultirotorApiBase const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); if (count_distance_sensors != 0) { const auto& distance_output = getDistanceSensorData(""); - float pitch, roll, yaw; - VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw); sendDistanceSensor(distance_output.min_distance / 100, //m -> cm distance_output.max_distance / 100, //m -> cm distance_output.distance, 0, //sensor type: //TODO: allow changing in settings? 77, //sensor id, //TODO: should this be something real? - pitch); //TODO: convert from radians to degrees? + distance_output.relative_pose.orientation); //TODO: convert from radians to degrees? } const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps); @@ -1379,7 +1377,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase last_sensor_message_ = hil_sensor; } - void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, float orientation) + void sendDistanceSensor(float min_distance, float max_distance, float current_distance, float sensor_type, float sensor_id, Quaternionr orientation) { if (!is_simulation_mode_) throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode"); @@ -1392,7 +1390,14 @@ class MavLinkMultirotorApi : public MultirotorApiBase distance_sensor.current_distance = static_cast(current_distance); distance_sensor.type = static_cast(sensor_type); distance_sensor.id = static_cast(sensor_id); - distance_sensor.orientation = static_cast(orientation); + + // Use custom orientation + distance_sensor.orientation = 100; // MAV_SENSOR_ROTATION_CUSTOM + distance_sensor.quaternion[0] = orientation.w(); + distance_sensor.quaternion[1] = orientation.x(); + distance_sensor.quaternion[2] = orientation.y(); + distance_sensor.quaternion[3] = orientation.z(); + //TODO: use covariance parameter? if (hil_node_ != nullptr) { diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp index 836f8943b2..6044e5e18d 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/mavlink/Px4MultiRotorParams.hpp @@ -57,192 +57,6 @@ class Px4MultiRotorParams : public MultiRotorParams { } private: - void setupFrameGenericQuad(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameGenericHex(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 6; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorHexX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameFlamewheel(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.225f); - - //set up mass - params.mass = 1.635f; - real_T motor_assembly_weight = 0.052f; - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - params.rotor_params.C_T = 0.11f; - params.rotor_params.C_P = 0.047f; - params.rotor_params.max_rpm = 9500; - params.rotor_params.calculateMaxThrust(); - params.linear_drag_coefficient *= 4; // make top speed more real. - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; - real_T rotor_z = 0.15f; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameFlamewheelFLA(Params& params) - { - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.225f); - - //set up mass - params.mass = 2.25f; - real_T motor_assembly_weight = 0.1f; - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - params.rotor_params.C_T = 0.2f; - params.rotor_params.C_P = 0.1f; - params.rotor_params.max_rpm = 9324; - params.rotor_params.calculateMaxThrust(); - params.linear_drag_coefficient *= 4; // make top speed more real. - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.16f; params.body_box.y() = 0.10f; params.body_box.z() = 0.14f; - real_T rotor_z = 0.15f; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - void setupFrameBlacksheep(Params& params) - { - /* - Motor placement: - x - (2) | (0) - | - ------------ y - | - (1) | (3) - | - - */ - //set up arm lengths - //dimensions are for Team Blacksheep Discovery (http://team-blacksheep.com/products/product:98) - params.rotor_count = 4; - std::vector arm_lengths; - - Vector3r unit_z(0, 0, -1); //NED frame - - // relative to Forward vector in the order (0,3,1,2) required by quad X pattern - // http://ardupilot.org/copter/_images/MOTORS_QuadX_QuadPlus.jpg - arm_lengths.push_back(0.22f); - arm_lengths.push_back(0.255f); - arm_lengths.push_back(0.22f); - arm_lengths.push_back(0.255f); - - // note: the Forward vector is actually the "x" axis, and the AngleAxisr rotation is pointing down and is left handed, so this means the rotation - // is counter clockwise, so the vector (arm_lengths[i], 0) is the X-axis, so the CCW rotations to position each arm correctly are listed below: - // See measurements here: http://diydrones.com/profiles/blogs/arducopter-tbs-discovery-style (angles reversed because we are doing CCW rotation) - std::vector arm_angles; - arm_angles.push_back(-55.0f); - arm_angles.push_back(125.0f); - arm_angles.push_back(55.0f); - arm_angles.push_back(-125.0f); - - // quad X pattern - std::vector rotor_directions; - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); - rotor_directions.push_back(RotorTurningDirection::RotorTurningDirectionCW); - - // data from - // http://dronesvision.net/team-blacksheep-750kv-motor-esc-set-for-tbs-discovery-fpv-quadcopter/ - //set up mass - params.mass = 2.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.052f; // weight for TBS motors - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // the props we are using a E-Prop, which I didn't find in UIUC database, but this one is close: - // http://m-selig.ae.illinois.edu/props/volume-2/plots/ef_130x70_static_ctcp.png - params.rotor_params.C_T = 0.11f; - params.rotor_params.C_P = 0.047f; - params.rotor_params.max_rpm = 9500; - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.20f; params.body_box.y() = 0.12f; params.body_box.z() = 0.04f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - params.rotor_poses.clear(); - for (uint i = 0; i < 4; i++) - { - Quaternionr angle(AngleAxisr(arm_angles[i] * M_PIf / 180, unit_z)); - params.rotor_poses.emplace_back(VectorMath::rotateVector(Vector3r(arm_lengths[i], 0, rotor_z), angle, true), unit_z, rotor_directions[i]); - }; - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - } - - static const AirSimSettings::MavLinkConnectionInfo& getConnectionInfo(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting) { return vehicle_setting.connection_info; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp index 29c0b24937..1fae801433 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightQuadXParams.hpp @@ -31,33 +31,10 @@ class SimpleFlightQuadXParams : public MultiRotorParams { { auto& params = getParams(); - /******* Below is same config as PX4 generic model ********/ + // Use connection_info_.model for the model name, see Px4MultiRotorParams for example - //set up arm lengths - //dimensions are for F450 frame: http://artofcircuits.com/product/quadcopter-frame-hj450-with-power-distribution - params.rotor_count = 4; - std::vector arm_lengths(params.rotor_count, 0.2275f); - - //set up mass - params.mass = 1.0f; //can be varied from 0.800 to 1.600 - real_T motor_assembly_weight = 0.055f; //weight for MT2212 motor for F450 frame - real_T box_mass = params.mass - params.rotor_count * motor_assembly_weight; - - // using rotor_param default, but if you want to change any of the rotor_params, call calculateMaxThrust() to recompute the max_thrust - // given new thrust coefficients, motor max_rpm and propeller diameter. - params.rotor_params.calculateMaxThrust(); - - //set up dimensions of core body box or abdomen (not including arms). - params.body_box.x() = 0.180f; params.body_box.y() = 0.11f; params.body_box.z() = 0.040f; - real_T rotor_z = 2.5f / 100; - - //computer rotor poses - initializeRotorQuadX(params.rotor_poses, params.rotor_count, arm_lengths.data(), rotor_z); - - //compute inertia matrix - computeInertiaMatrix(params.inertia, params.body_box, params.rotor_poses, box_mass, motor_assembly_weight); - - //leave everything else to defaults + // Only Generic for now + setupFrameGenericQuad(params); } virtual const SensorFactory* getSensorFactory() const override @@ -66,7 +43,6 @@ class SimpleFlightQuadXParams : public MultiRotorParams { } private: - vector> sensor_storage_; const AirSimSettings::VehicleSetting* vehicle_setting_; //store as pointer because of derived classes std::shared_ptr sensor_factory_; }; diff --git a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp index 69128e6a6d..fdc53880a9 100644 --- a/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp +++ b/AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/PidController.hpp @@ -99,7 +99,7 @@ class PidController : public IUpdatable { if (dt > min_dt_) { integrator->update(dt, error, last_time_); - float error_der = (error - last_error_) / dt; + float error_der = dt > 0 ? (error - last_error_) / dt : 0; dterm = error_der * config_.kd; last_error_ = error; } diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 35765f3549..1c71a4ab41 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -184,7 +184,7 @@ msr::airlib::GpsBase::Output RpcLibClientBase::getGpsData(const std::string& gps return pimpl_->client.call("getGpsData", gps_name, vehicle_name).as().to(); } -msr::airlib::DistanceBase::Output RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const +msr::airlib::DistanceSensorData RpcLibClientBase::getDistanceSensorData(const std::string& distance_sensor_name, const std::string& vehicle_name) const { return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as().to(); } @@ -346,23 +346,41 @@ std::vector RpcLibClientBase::simSwapTextures(const std::string& ta return pimpl_->client.call("simSwapTextures", tags, tex_id, component_id, material_id).as>(); } +bool RpcLibClientBase::simLoadLevel(const string& level_name) +{ + return pimpl_->client.call("simLoadLevel", level_name).as(); +} + +msr::airlib::Vector3r RpcLibClientBase::simGetObjectScale(const std::string& object_name) const +{ + return pimpl_->client.call("simGetObjectScale", object_name).as().to(); +} + msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_name) const { return pimpl_->client.call("simGetObjectPose", object_name).as().to(); } + bool RpcLibClientBase::simSetObjectPose(const std::string& object_name, const msr::airlib::Pose& pose, bool teleport) { return pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose), teleport).as(); } +bool RpcLibClientBase::simSetObjectScale(const std::string& object_name, const msr::airlib::Vector3r& scale) +{ + return pimpl_->client.call("simSetObjectScale", object_name, RpcLibAdapatorsBase::Vector3r(scale)).as(); +} + CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const { return pimpl_->client.call("simGetCameraInfo", camera_name, vehicle_name).as().to(); } -void RpcLibClientBase::simSetCameraOrientation(const std::string& camera_name, const Quaternionr& orientation, const std::string& vehicle_name) + +void RpcLibClientBase::simSetCameraPose(const std::string& camera_name, const Pose& pose, const std::string& vehicle_name) { - pimpl_->client.call("simSetCameraOrientation", camera_name, RpcLibAdapatorsBase::Quaternionr(orientation), vehicle_name); + pimpl_->client.call("simSetCameraPose", camera_name, RpcLibAdapatorsBase::Pose(pose), vehicle_name); } + void RpcLibClientBase::simSetCameraFov(const std::string& camera_name, float fov_degrees, const std::string& vehicle_name) { pimpl_->client.call("simSetCameraFov", camera_name, fov_degrees, vehicle_name); @@ -394,6 +412,21 @@ RpcLibClientBase* RpcLibClientBase::waitOnLastTask(bool* task_result, float time return this; } +void RpcLibClientBase::startRecording() +{ + pimpl_->client.call("startRecording"); +} + +void RpcLibClientBase::stopRecording() +{ + pimpl_->client.call("stopRecording"); +} + +bool RpcLibClientBase::isRecording() +{ + return pimpl_->client.call("isRecording").as(); +} + void* RpcLibClientBase::getClient() { return &pimpl_->client; diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 8345cb2017..bdf256c0d6 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -83,9 +83,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_.reset(new impl(server_address, port)); pimpl_->server.bind("ping", [&]() -> bool { return true; }); + pimpl_->server.bind("getServerVersion", []() -> int { return 1; }); + pimpl_->server.bind("getMinRequiredClientVersion", []() -> int { return 1; }); @@ -93,9 +95,11 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simPause", [&](bool is_paused) -> void { getWorldSimApi()->pause(is_paused); }); + pimpl_->server.bind("simIsPaused", [&]() -> bool { return getWorldSimApi()->isPaused(); }); + pimpl_->server.bind("simContinueForTime", [&](double seconds) -> void { getWorldSimApi()->continueForTime(seconds); }); @@ -109,6 +113,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("simEnableWeather", [&](bool enable) -> void { getWorldSimApi()->enableWeather(enable); }); + pimpl_->server.bind("simSetWeatherParameter", [&](WorldSimApiBase::WeatherParameter param, float val) -> void { getWorldSimApi()->setWeatherParameter(param, val); }); @@ -116,17 +121,21 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& pimpl_->server.bind("enableApiControl", [&](bool is_enabled, const std::string& vehicle_name) -> void { getVehicleApi(vehicle_name)->enableApiControl(is_enabled); }); + pimpl_->server.bind("isApiControlEnabled", [&](const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->isApiControlEnabled(); }); + pimpl_->server.bind("armDisarm", [&](bool arm, const std::string& vehicle_name) -> bool { return getVehicleApi(vehicle_name)->armDisarm(arm); }); + pimpl_->server.bind("simGetImages", [&](const std::vector& request_adapter, const std::string& vehicle_name) -> vector { const auto& response = getVehicleSimApi(vehicle_name)->getImages(RpcLibAdapatorsBase::ImageRequest::to(request_adapter)); return RpcLibAdapatorsBase::ImageResponse::from(response); }); + pimpl_->server.bind("simGetImage", [&](const std::string& camera_name, ImageCaptureBase::ImageType type, const std::string& vehicle_name) -> vector { auto result = getVehicleSimApi(vehicle_name)->getImage(camera_name, type); if (result.size() == 0) { @@ -145,6 +154,7 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& bind("simSetVehiclePose", [&](const RpcLibAdapatorsBase::Pose &pose, bool ignore_collision, const std::string& vehicle_name) -> void { getVehicleSimApi(vehicle_name)->setPose(pose.to(), ignore_collision); }); + pimpl_->server.bind("simGetVehiclePose", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getVehicleSimApi(vehicle_name)->getPose(); return RpcLibAdapatorsBase::Pose(pose); @@ -176,7 +186,8 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& sim_world_api->reset(); else getVehicleApi("")->reset(); - resetInProgress = false; + + resetInProgress = false; }); pimpl_->server.bind("simPrintLogMessage", [&](const std::string& message, const std::string& message_param, unsigned char severity) -> void { @@ -223,9 +234,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return RpcLibAdapatorsBase::CameraInfo(camera_info); }); - pimpl_->server.bind("simSetCameraOrientation", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Quaternionr& orientation, + pimpl_->server.bind("simSetCameraPose", [&](const std::string& camera_name, const RpcLibAdapatorsBase::Pose& pose, const std::string& vehicle_name) -> void { - getVehicleSimApi(vehicle_name)->setCameraOrientation(camera_name, orientation.to()); + getVehicleSimApi(vehicle_name)->setCameraPose(camera_name, pose.to()); }); pimpl_->server.bind("simSetCameraFov", [&](const std::string& camera_name, float fov_degrees, @@ -242,32 +253,58 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& return getWorldSimApi()->listSceneObjects(name_regex); }); + pimpl_->server.bind("simLoadLevel", [&](const std::string& level_name) -> bool { + return getWorldSimApi()->loadLevel(level_name); + }); + + pimpl_->server.bind("simSpawnObject", [&](string& object_name, const string& load_component, const RpcLibAdapatorsBase::Pose& pose, const RpcLibAdapatorsBase::Vector3r& scale) -> string { + return getWorldSimApi()->spawnObject(object_name, load_component, pose.to(), scale.to()); + }); + + pimpl_->server.bind("simDestroyObject", [&](const string& object_name) -> bool { + return getWorldSimApi()->destroyObject(object_name); + }); + pimpl_->server.bind("simGetObjectPose", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Pose { const auto& pose = getWorldSimApi()->getObjectPose(object_name); return RpcLibAdapatorsBase::Pose(pose); }); + + pimpl_->server.bind("simGetObjectScale", [&](const std::string& object_name) -> RpcLibAdapatorsBase::Vector3r { + const auto& scale = getWorldSimApi()->getObjectScale(object_name); + return RpcLibAdapatorsBase::Vector3r(scale); + }); + pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose, bool teleport) -> bool { return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport); }); + pimpl_->server.bind("simSetObjectScale", [&](const std::string& object_name, const RpcLibAdapatorsBase::Vector3r& scale) -> bool { + return getWorldSimApi()->setObjectScale(object_name, scale.to()); + }); + pimpl_->server.bind("simFlushPersistentMarkers", [&]() -> void { getWorldSimApi()->simFlushPersistentMarkers(); }); + pimpl_->server.bind("simPlotPoints", [&](const std::vector& points, const vector& color_rgba, float size, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotPoints(conv_points, color_rgba, size, duration, is_persistent); }); + pimpl_->server.bind("simPlotLineStrip", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotLineStrip(conv_points, color_rgba, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotLineList", [&](const std::vector& points, const vector& color_rgba, float thickness, float duration, bool is_persistent) -> void { vector conv_points; RpcLibAdapatorsBase::to(points, conv_points); getWorldSimApi()->simPlotLineList(conv_points, color_rgba, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotArrows", [&](const std::vector& points_start, const std::vector& points_end, const vector& color_rgba, float thickness, float arrow_size, float duration, bool is_persistent) -> void { vector conv_points_start; RpcLibAdapatorsBase::to(points_start, conv_points_start); @@ -275,21 +312,25 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& RpcLibAdapatorsBase::to(points_end, conv_points_end); getWorldSimApi()->simPlotArrows(conv_points_start, conv_points_end, color_rgba, thickness, arrow_size, duration, is_persistent); }); + pimpl_->server.bind("simPlotStrings", [&](const std::vector strings, const std::vector& positions, float scale, const vector& color_rgba, float duration) -> void { vector conv_positions; RpcLibAdapatorsBase::to(positions, conv_positions); getWorldSimApi()->simPlotStrings(strings, conv_positions, scale, color_rgba, duration); }); + pimpl_->server.bind("simPlotTransforms", [&](const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) -> void { vector conv_poses; RpcLibAdapatorsBase::to(poses, conv_poses); getWorldSimApi()->simPlotTransforms(conv_poses, scale, thickness, duration, is_persistent); }); + pimpl_->server.bind("simPlotTransformsWithNames", [&](const std::vector& poses, const std::vector names, float tf_scale, float tf_thickness, float text_scale, const vector& text_color_rgba, float duration) -> void { vector conv_poses; RpcLibAdapatorsBase::to(poses, conv_poses); getWorldSimApi()->simPlotTransformsWithNames(conv_poses, names, tf_scale, tf_thickness, text_scale, text_color_rgba, duration); }); + pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState { const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics(); return RpcLibAdapatorsBase::KinematicsState(result); @@ -304,9 +345,21 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getVehicleApi(vehicle_name)->cancelLastTask(); }); - pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { - return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); - }); + pimpl_->server.bind("simSwapTextures", [&](const std::string tag, int tex_id, int component_id, int material_id) -> std::vector { + return *getWorldSimApi()->swapTextures(tag, tex_id, component_id, material_id); + }); + + pimpl_->server.bind("startRecording", [&]() -> void { + getWorldSimApi()->startRecording(); + }); + + pimpl_->server.bind("stopRecording", [&]() -> void { + getWorldSimApi()->stopRecording(); + }); + + pimpl_->server.bind("isRecording", [&]() -> bool { + return getWorldSimApi()->isRecording(); + }); //if we don't suppress then server will bomb out for exceptions raised by any method pimpl_->server.suppress_exceptions(true); diff --git a/AirLib/src/common/common_utils/FileSystem.cpp b/AirLib/src/common/common_utils/FileSystem.cpp index 176370ae3a..bf37e55a70 100644 --- a/AirLib/src/common/common_utils/FileSystem.cpp +++ b/AirLib/src/common/common_utils/FileSystem.cpp @@ -43,7 +43,7 @@ #include #endif -using namespace common_utils; +namespace common_utils { // File names are unicode (std::wstring), because users can create folders containing unicode characters on both // Windows, OSX and Linux. @@ -93,8 +93,6 @@ std::string FileSystem::getUserDocumentsFolder() { return ensureFolder(path); } -#endif - std::string FileSystem::getExecutableFolder() { std::string path; #ifdef _WIN32 @@ -135,4 +133,8 @@ std::string FileSystem::getExecutableFolder() { path = path.substr(0, pathSeparatorIndex); return ensureFolder(path); -} \ No newline at end of file +} + +} // namespace common_utils + +#endif \ No newline at end of file diff --git a/AirLibUnitTests/TestBase.hpp b/AirLibUnitTests/TestBase.hpp index 13342dd2c2..d1916fb963 100644 --- a/AirLibUnitTests/TestBase.hpp +++ b/AirLibUnitTests/TestBase.hpp @@ -9,6 +9,7 @@ namespace msr { namespace airlib { class TestBase { public: + virtual ~TestBase() = default; virtual void run() = 0; void testAssert(double lhs, double rhs, const std::string& message) { diff --git a/MavLinkCom/MavLinkTest/Commands.h b/MavLinkCom/MavLinkTest/Commands.h index 3113f60edb..c05f51a6ea 100644 --- a/MavLinkCom/MavLinkTest/Commands.h +++ b/MavLinkCom/MavLinkTest/Commands.h @@ -691,7 +691,7 @@ class PidController proportionalGain = error * kProportional_; } if (kDerivative_ != 0) { - float derivative = (error - previous_error_) / dt; + float derivative = dt > 0 ? (error - previous_error_) / dt : 0; derivativeGain = derivative * kDerivative_; } if (kIntegral_ != 0) { diff --git a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp index 741e3b8cc9..2e4655b7bc 100644 --- a/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkConnectionImpl.cpp @@ -430,8 +430,6 @@ void MavLinkConnectionImpl::readPackets() } else if (frame_state == MAVLINK_FRAMING_OK) { - int msgId = msg.msgid; - // pick up the sysid/compid of the remote node we are connected to. if (other_system_id == -1) { other_system_id = msg.sysid; diff --git a/MavLinkCom/src/impl/MavLinkNodeImpl.cpp b/MavLinkCom/src/impl/MavLinkNodeImpl.cpp index 7fe2740da1..4772c747fe 100644 --- a/MavLinkCom/src/impl/MavLinkNodeImpl.cpp +++ b/MavLinkCom/src/impl/MavLinkNodeImpl.cpp @@ -592,8 +592,9 @@ void MavLinkNodeImpl::sendCommand(MavLinkCommand& command) try { sendMessage(cmd); } - catch (std::exception e) { + catch (const std::exception& e) { // silently fail since we are on a background thread here... + unused(e); } } diff --git a/MavLinkCom/src/impl/MavLinkNodeImpl.hpp b/MavLinkCom/src/impl/MavLinkNodeImpl.hpp index b30a87bc8b..295c3f2e6a 100644 --- a/MavLinkCom/src/impl/MavLinkNodeImpl.hpp +++ b/MavLinkCom/src/impl/MavLinkNodeImpl.hpp @@ -15,7 +15,7 @@ namespace mavlinkcom_impl { { public: MavLinkNodeImpl(int localSystemId, int localComponentId); - ~MavLinkNodeImpl(); + virtual ~MavLinkNodeImpl(); void connect(std::shared_ptr connection); void close(); diff --git a/MavLinkCom/src/serial_com/SerialPort.hpp b/MavLinkCom/src/serial_com/SerialPort.hpp index 49101d36fd..9ed32d5d16 100644 --- a/MavLinkCom/src/serial_com/SerialPort.hpp +++ b/MavLinkCom/src/serial_com/SerialPort.hpp @@ -37,7 +37,7 @@ class SerialPort : public Port { public: SerialPort(); - ~SerialPort(); + virtual ~SerialPort(); // open the serial port virtual int connect(const char* portName, int baudRate); diff --git a/MavLinkCom/src/serial_com/TcpClientPort.cpp b/MavLinkCom/src/serial_com/TcpClientPort.cpp index 502f02eb25..10568ba6ac 100644 --- a/MavLinkCom/src/serial_com/TcpClientPort.cpp +++ b/MavLinkCom/src/serial_com/TcpClientPort.cpp @@ -227,7 +227,6 @@ class TcpClientPort::TcpSocketImpl // write to the serial port int write(const uint8_t* ptr, int count) { - socklen_t addrlen = sizeof(sockaddr_in); int hr = send(sock, reinterpret_cast(ptr), count, 0); if (hr == SOCKET_ERROR) { @@ -262,7 +261,6 @@ class TcpClientPort::TcpSocketImpl while (!closed_) { - socklen_t addrlen = sizeof(sockaddr_in); int rc = recv(sock, reinterpret_cast(result), bytesToRead, 0); if (rc < 0) { diff --git a/MavLinkCom/src/serial_com/TcpClientPort.hpp b/MavLinkCom/src/serial_com/TcpClientPort.hpp index ee1ca733f9..bcca77138d 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 9908ee5e66..0c83c494ad 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 f1aaf7c10a..44034af244 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -417,17 +417,17 @@ def simGetCameraInfo(self, camera_name, vehicle_name = ''): # TODO: below str() conversion is only needed for legacy reason and should be removed in future return CameraInfo.from_msgpack(self.client.call('simGetCameraInfo', str(camera_name), vehicle_name)) - def simSetCameraOrientation(self, camera_name, orientation, vehicle_name = ''): + def simSetCameraPose(self, camera_name, pose, vehicle_name = ''): """ - - Control the orientation of a selected camera + - Control the pose of a selected camera Args: camera_name (str): Name of the camera to be controlled - orientation (Quaternionr): Quaternion representing the desired orientation of the camera + pose (Pose): Pose representing the desired position and orientation of the camera vehicle_name (str, optional): Name of vehicle which the camera corresponds to """ # TODO: below str() conversion is only needed for legacy reason and should be removed in future - self.client.call('simSetCameraOrientation', str(camera_name), orientation, vehicle_name) + self.client.call('simSetCameraPose', str(camera_name), pose, vehicle_name) def simSetCameraFov(self, camera_name, fov_degrees, vehicle_name = ''): """ @@ -677,6 +677,30 @@ def waitOnLastTask(self, timeout_sec = float('nan')): """ return self.client.call('waitOnLastTask', timeout_sec) + # Recording APIs + def startRecording(self): + """ + Start Recording + + Recording will be done according to the settings + """ + self.client.call('startRecording') + + def stopRecording(self): + """ + Stop Recording + """ + self.client.call('stopRecording') + + def isRecording(self): + """ + Whether Recording is running or not + + Returns: + bool: True if Recording, else False + """ + return self.client.call('isRecording') + # ----------------------------------- Multirotor APIs --------------------------------------------- class MultirotorClient(VehicleClient, object): def __init__(self, ip = "", port = 41451, timeout_value = 3600): diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 54e6d306b4..2f214efff1 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -401,9 +401,9 @@ class GpsData(MsgpackMixin): class DistanceSensorData(MsgpackMixin): time_stamp = np.uint64(0) - distance = Quaternionr() - min_distance = Quaternionr() - max_distance = Quaternionr() + distance = 0.0 + min_distance = 0.0 + max_distance = 0.0 relative_pose = Pose() class PIDGains(): diff --git a/PythonClient/car/distance_sensor_multi.py b/PythonClient/car/distance_sensor_multi.py new file mode 100644 index 0000000000..5e4660ba69 --- /dev/null +++ b/PythonClient/car/distance_sensor_multi.py @@ -0,0 +1,49 @@ +import airsim +import time + +''' +An example script showing usage of Distance sensor to measure distance between 2 Car vehicles +Settings - + +{ + "SettingsVersion": 1.2, + "SimMode": "Car", + "Vehicles": { + "Car1": { + "VehicleType": "PhysXCar", + "AutoCreate": true, + "Sensors": { + "Distance": { + "SensorType": 5, + "Enabled" : true, + "DrawDebugPoints": true + } + } + }, + "Car2": { + "VehicleType": "PhysXCar", + "AutoCreate": true, + "X": 10, "Y": 0, "Z": 0, + "Sensors": { + "Distance": { + "SensorType": 5, + "Enabled" : true, + "DrawDebugPoints": true + } + } + } + } +} + +Car2 is placed in front of Car 1 + +''' + +client = airsim.CarClient() +client.confirmConnection() + +while True: + data_car1 = client.getDistanceSensorData(vehicle_name="Car1") + data_car2 = client.getDistanceSensorData(vehicle_name="Car2") + print(f"Distance sensor data: Car1: {data_car1.distance}, Car2: {data_car2.distance}") + time.sleep(1.0) \ No newline at end of file diff --git a/PythonClient/computer_vision/cv_mode.py b/PythonClient/computer_vision/cv_mode.py index 188080ba2d..0691c0de7e 100644 --- a/PythonClient/computer_vision/cv_mode.py +++ b/PythonClient/computer_vision/cv_mode.py @@ -13,8 +13,9 @@ client = airsim.VehicleClient() client.confirmConnection() -airsim.wait_key('Press any key to set camera-0 gimble to 15-degree pitch') -client.simSetCameraOrientation("0", airsim.to_quaternion(0.261799, 0, 0)); #radians +airsim.wait_key('Press any key to set camera-0 gimbal to 15-degree pitch') +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #radians +client.simSetCameraPose("0", camera_pose) airsim.wait_key('Press any key to get camera parameters') for camera_name in range(5): @@ -48,4 +49,4 @@ time.sleep(3) # currently reset() doesn't work in CV mode. Below is the workaround -client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) \ No newline at end of file +client.simSetPose(airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0, 0, 0)), True) diff --git a/PythonClient/multirotor/gimbal.py b/PythonClient/multirotor/gimbal.py index fef1aeb527..45beb6a232 100644 --- a/PythonClient/multirotor/gimbal.py +++ b/PythonClient/multirotor/gimbal.py @@ -17,7 +17,9 @@ for i in range(5): client.moveToPositionAsync(float(-50.00), float( 50.26), float( -20.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(0.5, 0.5, 0.1)) + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.5, 0.5, 0.1)) + client.simSetCameraPose("0", camera_pose) client.moveToPositionAsync(float(50.00), float( -50.26), float( -10.58), float( 3.5)) time.sleep(6) - client.simSetCameraOrientation("0", airsim.to_quaternion(-0.5, -0.5, -0.1)) \ No newline at end of file + camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(-0.5, -0.5, -0.1)) + client.simSetCameraPose("0", camera_pose) diff --git a/README.md b/README.md index ae9bdafd31..663b901e8c 100644 --- a/README.md +++ b/README.md @@ -34,11 +34,11 @@ For complete list of changes, view our [Changelog](docs/CHANGELOG.md) ## How to Get It ### Windows -* [Download binaries](https://microsoft.github.io/AirSim/use_precompiled) +* [Download binaries](https://github.com/Microsoft/AirSim/releases) * [Build it](https://microsoft.github.io/AirSim/build_windows) ### Linux -* [Download binaries](https://microsoft.github.io/AirSim/use_precompiled) +* [Download binaries](https://github.com/Microsoft/AirSim/releases) * [Build it](https://microsoft.github.io/AirSim/build_linux) ### macOS diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp index 4a698b8356..9c45287d57 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.cpp @@ -11,7 +11,7 @@ bool(*SetEnableApi)(bool enableApi, const char* vehicleName); bool(*SetCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName); AirSimCarState(*GetCarState)(const char* vehicleName); AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName); -bool(*SetCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName); +bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName); bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName); bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex); int(*GetSegmentationObjectId)(const char* meshName); @@ -33,7 +33,7 @@ void InitVehicleManager( bool(*setCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName), AirSimCarState(*getCarState)(const char* vehicleName), AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName), - bool(*setCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName), + bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName), bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName), bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex), int(*getSegmentationObjectId)(const char* meshName), @@ -55,7 +55,7 @@ void InitVehicleManager( SetCarApiControls = setCarApiControls; GetCarState = getCarState; GetCameraInfo = getCameraInfo; - SetCameraOrientation = setCameraOrientation; + SetCameraPose = setCameraPose; SetCameraFoV = setCameraFoV; SetSegmentationObjectId = setSegmentationObjectId; GetSegmentationObjectId = getSegmentationObjectId; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h index 9bb45c54b8..36c22c2e1e 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PInvokeWrapper.h @@ -25,7 +25,7 @@ extern bool(*SetEnableApi)(bool enableApi, const char* vehicleName); extern bool(*SetCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName); extern AirSimCarState(*GetCarState)(const char* vehicleName); extern AirSimCameraInfo(*GetCameraInfo)(const char* cameraName, const char* vehicleName); -extern bool(*SetCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName); +extern bool(*SetCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName); extern bool(*SetCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName); extern bool(*SetSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex); extern int(*GetSegmentationObjectId)(const char* meshName); @@ -49,7 +49,7 @@ extern "C" EXPORT void InitVehicleManager( bool(*setCarApiControls)(msr::airlib::CarApiBase::CarControls controls, const char* vehicleName), AirSimCarState(*getCarState)(const char* vehicleName), AirSimCameraInfo(*getCameraInfo)(const char* cameraName, const char* vehicleName), - bool(*setCameraOrientation)(const char* cameraName, AirSimQuaternion orientation, const char* vehicleName), + bool(*setCameraPose)(const char* cameraName, AirSimPose pose, const char* vehicleName), bool(*setCameraFoV)(const char* cameraName, const float fov_degrees, const char* vehicleName), bool(*setSegmentationObjectId)(const char* meshName, int objectId, bool isNameRegex), int(*getSegmentationObjectId)(const char* meshName), diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp index cacadce82f..e4b5966142 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp @@ -210,15 +210,9 @@ msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name return camera_info; } -void PawnSimApi::setCameraOrientation(const std::string& camera_name, const msr::airlib::Quaternionr& orientation) +void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) { - AirSimQuaternion airSimOrientation; - airSimOrientation.x = orientation.x(); - airSimOrientation.y = orientation.y(); - airSimOrientation.z = orientation.z(); - airSimOrientation.w = orientation.w(); - - SetCameraOrientation(camera_name.c_str(), airSimOrientation, params_.vehicle_name.c_str()); + SetCameraPose(camera_name.c_str(), UnityUtilities::Convert_to_AirSimPose(pose), params_.vehicle_name.c_str()); } void PawnSimApi::setCameraFoV(const std::string& camera_name, float fov_degrees) diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h index 0108cc01b5..6525666e88 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h @@ -72,7 +72,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) override; virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override; - virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) override; + virtual void setCameraPose(const std::string& camera_name, const Pose& pose) override; virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp index 823c7f052f..d352439ca0 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp @@ -74,6 +74,10 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con return UnityUtilities::Convert_to_Pose(airSimPose); } +msr::airlib::Vector3r WorldSimApi::getObjectScale(const std::string& object_name) const { return Vector3r(); } +msr::airlib::Vector3r WorldSimApi::getObjectScaleInternal(const std::string& object_name) const { return Vector3r(); } +bool WorldSimApi::setObjectScale(const std::string& object_name, const Vector3r& scale) { return false; } + bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimApi::Pose& pose, bool teleport) { AirSimUnity::AirSimPose airSimPose = UnityUtilities::Convert_to_AirSimPose(pose); @@ -149,4 +153,24 @@ std::vector WorldSimApi::getMesh return result; } +// Recording APIs +void WorldSimApi::startRecording() +{ + throw std::invalid_argument(common_utils::Utils::stringf( + "startRecording is not supported on unity").c_str()); +} + +void WorldSimApi::stopRecording() +{ + throw std::invalid_argument(common_utils::Utils::stringf( + "stopRecording is not supported on unity").c_str()); +} + +bool WorldSimApi::isRecording() const +{ + throw std::invalid_argument(common_utils::Utils::stringf( + "isRecording is not supported on unity").c_str()); + return false; +} + #pragma endregion diff --git a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h index 697a30e1b1..a947d7cf6b 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h +++ b/Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h @@ -13,6 +13,12 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase WorldSimApi(SimModeBase* simmode, std::string vehicle_name); virtual ~WorldSimApi(); + + // ------ Level setting apis ----- // + virtual bool loadLevel(const std::string& level_name) { return false; }; + virtual std::string spawnObject(std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale) { return ""; }; + virtual bool destroyObject(const std::string& object_name) { return false; }; + virtual bool isPaused() const override; virtual void reset() override; virtual void pause(bool is_paused) override; @@ -31,7 +37,11 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; virtual std::vector listSceneObjects(const std::string& name_regex) const override; virtual Pose getObjectPose(const std::string& object_name) const override; + + virtual Vector3r getObjectScale(const std::string& object_name) const override; + Vector3r getObjectScaleInternal(const std::string& object_name) const; virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) override; //----------- Plotting APIs ----------/ virtual void simFlushPersistentMarkers() override; @@ -44,6 +54,11 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase virtual void simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) override; virtual std::vector getMeshPositionVertexBuffers() const override; + // Recording APIs + virtual void startRecording() override; + virtual void stopRecording() override; + virtual bool isRecording() const override; + private: SimModeBase * simmode_; std::string vehicle_name_; diff --git a/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake b/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake index e1de801535..b637eab622 100644 --- a/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake +++ b/Unity/AirLibWrapper/AirsimWrapper/cmake/rpc-setup.cmake @@ -125,7 +125,7 @@ if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") # clang is the compiler used for developing mainly, so # this is where I set the highest warning level list(APPEND RPCLIB_BUILD_FLAGS - -Wall -Wextra -Wno-c++98-compat + -Wall -Wextra -Wno-unused-function -Wno-c++98-compat -Wno-unknown-warning-option -Wno-c++98-compat-pedantic -Wno-padded -Wno-missing-prototypes -Wno-undef -pthread) @@ -268,7 +268,7 @@ if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") # clang is the compiler used for developing mainly, so # this is where I set the highest warning level list(APPEND RPCLIB_BUILD_FLAGS - -Wall -Wextra -Wno-c++98-compat + -Wall -Wextra -Wno-unused-function -Wno-c++98-compat -Wno-unknown-warning-option -Wno-c++98-compat-pedantic -Wno-padded -Wno-missing-prototypes -Wno-undef -pthread) diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs index 41121e65cd..1a970a7227 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/DataCaptureScript.cs @@ -23,7 +23,7 @@ public class DataCaptureScript : MonoBehaviour { private Texture2D screenShot; private Rect captureRect; private AirSimPose currentPose; - private Quaternion poseFromAirLib; + private Pose poseFromAirLib; private bool isCapturing; private bool isPoseOverride; @@ -40,7 +40,8 @@ private void FixedUpdate() { DataManager.SetToAirSim(transform.rotation, ref currentPose.orientation); if (isPoseOverride) { - transform.rotation = poseFromAirLib; + transform.position = poseFromAirLib.position; + transform.rotation = poseFromAirLib.rotation; isPoseOverride = false; } } @@ -89,8 +90,9 @@ public AirSimPose GetPose() { return currentPose; } - public void SetOrientation(AirSimQuaternion orientation) { - DataManager.SetToUnity(orientation, ref poseFromAirLib); + public void SetPose(AirSimPose pose) { + DataManager.SetToUnity(pose.position, ref poseFromAirLib.position); + DataManager.SetToUnity(pose.orientation, ref poseFromAirLib.rotation); isPoseOverride = true; } diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs index ac1ec3deb8..8f156909ce 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Utilities/PInvokeWrapper.cs @@ -13,7 +13,7 @@ public static class PInvokeWrapper { [DllImport(DLL_NAME)] public static extern void InitVehicleManager(IntPtr SetPose, IntPtr GetPose, IntPtr GetCollisionInfo, IntPtr GetRCData, IntPtr GetSimImages, IntPtr SetRotorSpeed, IntPtr SetEnableApi, IntPtr SetCarApiControls, IntPtr GetCarState, - IntPtr GetCameraInfo, IntPtr SetCameraOrientation, IntPtr SetCameraFoV, IntPtr SetSegmentationObjectid, IntPtr GetSegmentationObjectId, + IntPtr GetCameraInfo, IntPtr SetCameraPose, IntPtr SetCameraFoV, IntPtr SetSegmentationObjectid, IntPtr GetSegmentationObjectId, IntPtr PrintLogMessage, IntPtr GetTransformFromUnity, IntPtr Reset, IntPtr GetVelocity, IntPtr GetRayCastHit, IntPtr Pause); [DllImport(DLL_NAME)] diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs index 7a865ddee4..2809319985 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/IVehicleInterface.cs @@ -33,7 +33,7 @@ public interface IVehicleInterface { CameraInfo GetCameraInfo(string cameraName); - bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation); + bool SetCameraPose(string cameraName, AirSimPose pose); bool SetCameraFoV(string cameraName, float fov_degrees); diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs index 488b80fdc3..2049d2eecd 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/Vehicle.cs @@ -264,10 +264,10 @@ public CameraInfo GetCameraInfo(string cameraName) { return info; } - public bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation) { + public bool SetCameraPose(string cameraName, AirSimPose pose) { foreach (DataCaptureScript capture in captureCameras) { if (capture.GetCameraName() == cameraName) { - capture.SetOrientation(orientation); + capture.SetPose(pose); return true; } } diff --git a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs index 20015e93ff..ce674191e7 100644 --- a/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs +++ b/Unity/UnityDemo/Assets/AirSimAssets/Scripts/Vehicles/VehicleCompanion.cs @@ -93,7 +93,7 @@ private static void InitDelegators() { Marshal.GetFunctionPointerForDelegate(new Func(SetCarApiControls)), Marshal.GetFunctionPointerForDelegate(new Func(GetCarState)), Marshal.GetFunctionPointerForDelegate(new Func(GetCameraInfo)), - Marshal.GetFunctionPointerForDelegate(new Func(SetCameraOrientation)), + Marshal.GetFunctionPointerForDelegate(new Func(SetCameraPose)), Marshal.GetFunctionPointerForDelegate(new Func(SetCameraFoV)), Marshal.GetFunctionPointerForDelegate(new Func(SetSegmentationObjectId)), Marshal.GetFunctionPointerForDelegate(new Func(GetSegmentationObjectId)), @@ -184,9 +184,9 @@ private static CameraInfo GetCameraInfo(string cameraName, string vehicleName) { return vehicle.VehicleInterface.GetCameraInfo(cameraName); } - private static bool SetCameraOrientation(string cameraName, AirSimQuaternion orientation, string vehicleName) { + private static bool SetCameraPose(string cameraName, AirSimPose pose, string vehicleName) { var vehicle = Vehicles.Find(element => element.vehicleName == vehicleName); - return vehicle.VehicleInterface.SetCameraOrientation(cameraName, orientation); + return vehicle.VehicleInterface.SetCameraPose(cameraName, pose); } private static bool SetCameraFoV(string cameraName, float fov_degrees, string vehicleName) { diff --git a/Unity/build.sh b/Unity/build.sh index d6a945d714..7392027669 100755 --- a/Unity/build.sh +++ b/Unity/build.sh @@ -1,7 +1,7 @@ #! /bin/bash set -x -# set -e +set -e # check for rpclib if [ ! -f ../external/rpclib/rpclib-2.2.1/rpclib.pc.in ]; then diff --git a/Unreal/Environments/Blocks/update_from_git_ci.bat b/Unreal/Environments/Blocks/update_from_git_ci.bat new file mode 100644 index 0000000000..58c565c838 --- /dev/null +++ b/Unreal/Environments/Blocks/update_from_git_ci.bat @@ -0,0 +1,34 @@ +@echo off +REM //---------- set up variable ---------- +setlocal +set ROOT_DIR=%~dp0 + +set AirSimPath=%1 + +REM default path works for Blocks environment +if "%AirSimPath%"=="" set "AirSimPath=..\..\.." + +IF NOT EXIST "%AirSimPath%" ( + echo "AirSimPath %AirSimPath% was not found" + goto :failed +) + +echo Using AirSimPath = %AirSimPath% + +robocopy /MIR "%AirSimPath%\Unreal\Plugins\AirSim" Plugins\AirSim /XD temp *. /njh /njs /ndl /np +robocopy /MIR "%AirSimPath%\AirLib" Plugins\AirSim\Source\AirLib /XD temp *. /njh /njs /ndl /np +robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.bat +robocopy /njh /njs /ndl /np "%AirSimPath%\Unreal\Environments\Blocks" "." *.sh +rem robocopy /njh /njs /ndl /np "%AirSimPath%" "." *.gitignore + +REM cmd /c clean.bat +REM cmd /c GenerateProjectFiles.bat + +goto :done + +:failed +echo Error occured while updating. +exit /b 1 + +:done +REM if "%1"=="" pause \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset b/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset new file mode 100644 index 0000000000..256ffb5bc1 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/Blueprints/BP_LoadingScreenWidget.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset new file mode 100644 index 0000000000..e2469f3564 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/BP_LevelLoadButton.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG new file mode 100644 index 0000000000..7cd3e4f6e9 Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.PNG differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset new file mode 100644 index 0000000000..131e0077ff Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/LoadingImages/Coastline2.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/NormalsMaterial.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/NormalsMaterial.uasset index 93973bb29b..a2a62d1c61 100644 Binary files a/Unreal/Plugins/AirSim/Content/HUDAssets/NormalsMaterial.uasset and b/Unreal/Plugins/AirSim/Content/HUDAssets/NormalsMaterial.uasset differ diff --git a/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset b/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset new file mode 100644 index 0000000000..ed11301a3b Binary files /dev/null and b/Unreal/Plugins/AirSim/Content/HUDAssets/OptionsMenu.uasset differ diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp index e1f62ce1f3..58a8ea4f53 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.cpp @@ -9,9 +9,10 @@ #include "Components/StaticMeshComponent.h" #include "EngineUtils.h" #include "Runtime/Engine/Classes/Engine/StaticMesh.h" -#include "UObject/UObjectIterator.h" +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" +#include "UObject/UObjectIterator.h" #include "Camera/CameraComponent.h" -//#include "Runtime/Foliage/Public/FoliageType.h" +#include "Runtime/Engine/Classes/GameFramework/PlayerStart.h" #include "Misc/MessageDialog.h" #include "Engine/LocalPlayer.h" #include "Engine/SkeletalMesh.h" @@ -19,6 +20,7 @@ #include "IImageWrapper.h" #include "Misc/ObjectThumbnail.h" #include "Engine/Engine.h" +#include "Engine/World.h" #include #include "common/common_utils/Utils.hpp" #include "Modules/ModuleManager.h" @@ -30,6 +32,7 @@ Methods -> CamelCase parameters -> camel_case */ +ULevelStreamingDynamic *UAirBlueprintLib::CURRENT_LEVEL = nullptr; bool UAirBlueprintLib::log_messages_hidden_ = false; msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType UAirBlueprintLib::mesh_naming_method_ = msr::airlib::AirSimSettings::SegmentationSetting::MeshNamingMethodType::OwnerName; @@ -73,6 +76,45 @@ void UAirBlueprintLib::setSimulatePhysics(AActor* actor, bool simulate_physics) } } +ULevelStreamingDynamic* UAirBlueprintLib::loadLevel(UObject* context, const FString& level_name) +{ + bool success{ false }; + context->GetWorld()->SetNewWorldOrigin(FIntVector(0,0,0)); + ULevelStreamingDynamic* new_level = UAirsimLevelStreaming::LoadAirsimLevelInstance( + context->GetWorld(), level_name, FVector(0, 0, 0), FRotator(0, 0, 0), success); + if (success) + { + if(CURRENT_LEVEL != nullptr && CURRENT_LEVEL->IsValidLowLevel()) + CURRENT_LEVEL->SetShouldBeLoaded(false); + CURRENT_LEVEL = new_level; + } + return CURRENT_LEVEL; +} + +bool UAirBlueprintLib::spawnPlayer(UWorld* context) +{ + + bool success{ false }; + TArray player_start_actors; + FindAllActor(context, player_start_actors); + if (player_start_actors.Num() > 1) + { + for (auto player_start : player_start_actors) + { + if (player_start->GetName() != FString("SuperStart")) + { + //context->GetWorld()->SetNewWorldOrigin(FIntVector(0, 0, 0)); + auto location = player_start->GetActorLocation(); + context->RequestNewWorldOrigin(FIntVector(location.X, location.Y, location.Z)); + success = true; + break; + } + } + } + return success; +} + + std::vector UAirBlueprintLib::getPhysicsComponents(AActor* actor) { std::vector phys_comps; @@ -324,14 +366,33 @@ bool UAirBlueprintLib::SetMeshStencilID(const std::string& mesh_name, int object int UAirBlueprintLib::GetMeshStencilID(const std::string& mesh_name) { - FString fmesh_name(mesh_name.c_str()); - for (TObjectIterator comp; comp; ++comp) - { - // Access the subclass instance with the * or -> operators. - UMeshComponent *mesh = *comp; - if (mesh->GetName() == fmesh_name) { + // Takes a UStaticMeshComponent, USkinnedMeshComponent or ALandscapeProxy and returns their custom stencil ID if + // their meshes's name or their owner's name (depending on the naming method in mesh_naming_method_) equals mesh_name + auto getCustomStencilForMesh = [&mesh_name](auto mesh) -> int { + const std::string component_mesh_name = common_utils::Utils::toLower(GetMeshName(mesh)); + if (component_mesh_name.compare(mesh_name) == 0) { return mesh->CustomDepthStencilValue; } + return -1; + }; + + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if(id != -1) + return id; + } + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if (id != -1) + return id; + } + for (TObjectIterator comp; comp; ++comp) + { + int id = getCustomStencilForMesh(*comp); + if (id != -1) + return id; } return -1; @@ -474,6 +535,46 @@ std::vector UAirBlueprintLib::Ge return meshes; } + +TArray UAirBlueprintLib::ListWorldsInRegistry() +{ + FARFilter Filter; + Filter.ClassNames.Add(UWorld::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + TArray WorldNames; + for (auto asset : AssetData) + WorldNames.Add(asset.AssetName); + return WorldNames; +} + +UObject* UAirBlueprintLib::GetMeshFromRegistry(const std::string& load_object) +{ + FARFilter Filter; + Filter.ClassNames.Add(UStaticMesh::StaticClass()->GetFName()); + Filter.bRecursivePaths = true; + + TArray AssetData; + FAssetRegistryModule& AssetRegistryModule = FModuleManager::LoadModuleChecked("AssetRegistry"); + AssetRegistryModule.Get().GetAssets(Filter, AssetData); + + UObject* LoadObject = NULL; + for (auto asset : AssetData) + { + UE_LOG(LogTemp, Log, TEXT("Asset path: %s"), *asset.PackagePath.ToString()); + if (asset.AssetName == FName(load_object.c_str())) + { + LoadObject = asset.GetAsset(); + break; + } + } + return LoadObject; +} + bool UAirBlueprintLib::HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor, ECollisionChannel collision_channel) { FCollisionQueryParams trace_params; diff --git a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h index 17f01b9201..bddcff4dbb 100644 --- a/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h +++ b/Unreal/Plugins/AirSim/Source/AirBlueprintLib.h @@ -4,8 +4,10 @@ #pragma once #include "CoreMinimal.h" +#include "Runtime/AssetRegistry/Public/AssetRegistryModule.h" #include "GameFramework/Actor.h" #include "Components/InputComponent.h" +#include "EngineUtils.h" #include "GameFramework/PlayerInput.h" #include "IImageWrapperModule.h" #include "Kismet/BlueprintFunctionLibrary.h" @@ -15,13 +17,16 @@ #include "Kismet/GameplayStatics.h" #include "Kismet/KismetStringLibrary.h" #include "Engine/World.h" - #include "Runtime/Landscape/Classes/LandscapeComponent.h" +#include "Runtime/Engine/Classes/Kismet/GameplayStatics.h" +#include "AirsimLevelStreaming.h" +#include "Runtime/Core/Public/HAL/FileManager.h" #include "common/AirSimSettings.hpp" #include #include #include "AirBlueprintLib.generated.h" +class ULevelStreamingDynamic; UENUM(BlueprintType) enum class LogDebugLevel : uint8 { @@ -46,25 +51,21 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary UFUNCTION(BlueprintCallable, Category = "Utils") static void LogMessage(const FString &prefix, const FString &suffix, LogDebugLevel level, float persist_sec = 60); static float GetWorldToMetersScale(const AActor* context); - template static T* GetActorComponent(AActor* actor, FString name); template static T* FindActor(const UObject* context, FString name) { - TArray foundActors; - FindAllActor(context, foundActors); FName name_n = FName(*name); - - for (AActor* actor : foundActors) { - if (actor->ActorHasTag(name_n) || actor->GetName().Compare(name) == 0) { - return static_cast(actor); + for (TActorIterator It(context->GetWorld(), T::StaticClass()); It; ++It) + { + AActor* Actor = *It; + if (!Actor->IsPendingKill() && (Actor->ActorHasTag(name_n) || Actor->GetName().Compare(name) == 0)) + { + return static_cast(Actor); } } - - //UAirBlueprintLib::LogMessage(name + TEXT(" Actor not found!"), TEXT(""), LogDebugLevel::Failure); - return nullptr; } @@ -73,8 +74,17 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary { UGameplayStatics::GetAllActorsOfClass(context, T::StaticClass(), foundActors); } + + static ULevelStreamingDynamic *CURRENT_LEVEL; static std::vector ListMatchingActors(const UObject *context, const std::string& name_regex); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static ULevelStreamingDynamic* loadLevel(UObject* context, const FString& level_name); + UFUNCTION(BlueprintCallable, Category = "AirSim|LevelAPI") + static bool spawnPlayer(UWorld* context); + UFUNCTION(BlueprintPure, Category = "AirSim|LevelAPI") + static TArray ListWorldsInRegistry(); + static UObject* GetMeshFromRegistry(const std::string& load_object); static bool HasObstacle(const AActor* actor, const FVector& start, const FVector& end, const AActor* ignore_actor = nullptr, ECollisionChannel collision_channel = ECC_Visibility); @@ -182,9 +192,7 @@ class UAirBlueprintLib : public UBlueprintFunctionLibrary static void setUnrealClockSpeed(const AActor* context, float clock_speed); static IImageWrapperModule* getImageWrapperModule(); static void CompressImageArray(int32 width, int32 height, const TArray &src, TArray &dest); - - static std::vector GetStaticMeshComponents(); - + static std::vector GetStaticMeshComponents(); private: template static void InitializeObjectStencilID(T* mesh, bool ignore_existing = true) diff --git a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs index 6ba94e59c0..493c77bd0b 100644 --- a/Unreal/Plugins/AirSim/Source/AirSim.Build.cs +++ b/Unreal/Plugins/AirSim/Source/AirSim.Build.cs @@ -78,7 +78,7 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) bEnableExceptions = true; - PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); + PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore", "ImageWrapper", "RenderCore", "RHI", "AssetRegistry","PhysXVehicles", "PhysXVehicleLib", "PhysX", "APEX", "Landscape" }); PrivateDependencyModuleNames.AddRange(new string[] { "UMG", "Slate", "SlateCore" }); //suppress VC++ proprietary warnings @@ -90,7 +90,7 @@ public AirSim(ReadOnlyTargetRules Target) : base(Target) PublicIncludePaths.Add(Path.Combine(AirLibPath, "deps", "eigen3")); AddOSLibDependencies(Target); - SetupCompileMode(CompileMode.CppCompileWithRpc, Target); + SetupCompileMode(CompileMode.HeaderOnlyWithRpc, Target); } private void AddOSLibDependencies(ReadOnlyTargetRules Target) diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp new file mode 100644 index 0000000000..29f634c510 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.cpp @@ -0,0 +1,51 @@ +#pragma once + +#include "AirsimLevelStreaming.h" +#include "Engine/World.h" + +int32 UAirsimLevelStreaming::LevelInstanceId = 0; + + +UAirsimLevelStreaming* UAirsimLevelStreaming::LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess) +{ + if (!WorldContextObject) + { + return nullptr; + } + + // Check whether requested map exists, this could be very slow if LevelName is a short package name + FString LongPackageName; + bool success = FPackageName::SearchForPackageOnDisk(LevelName, &LongPackageName); + if (!success) +{ + bOutSuccess = false; + return nullptr; + } + + // Create Unique Name for sub-level package + const FString ShortPackageName = FPackageName::GetShortName(LongPackageName); + const FString PackagePath = FPackageName::GetLongPackagePath(LongPackageName); + FString UniqueLevelPackageName = PackagePath + TEXT("/") + WorldContextObject->StreamingLevelsPrefix + ShortPackageName; + UniqueLevelPackageName += TEXT("_LevelInstance_") + FString::FromInt(++LevelInstanceId); + + // Setup streaming level object that will load specified map + ULevelStreamingDynamic* level_pointer = NewObject(WorldContextObject, ULevelStreamingDynamic::StaticClass(), NAME_None, RF_Transient, NULL); + level_pointer->SetWorldAssetByPackageName(FName(*UniqueLevelPackageName)); + level_pointer->LevelColor = FColor::MakeRandomColor(); + level_pointer->SetShouldBeLoaded(true); + level_pointer->SetShouldBeVisible(true); + level_pointer->bShouldBlockOnLoad = true; + level_pointer->bInitiallyLoaded = true; + level_pointer->bInitiallyVisible = true; + + // Transform + level_pointer->LevelTransform = FTransform(Rotation, Location); + // Map to Load + level_pointer->PackageNameToLoad = FName(*LongPackageName); + // Add the new level to world. + WorldContextObject->AddStreamingLevel(level_pointer); + + bOutSuccess = true; + + return dynamic_cast (level_pointer); +} \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h new file mode 100644 index 0000000000..d45570127e --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/AirsimLevelStreaming.h @@ -0,0 +1,12 @@ +#pragma once +#include "Runtime/Engine/Classes/Engine/LevelStreamingDynamic.h" + + +class UAirsimLevelStreaming : public ULevelStreamingDynamic +{ +public: + static UAirsimLevelStreaming* LoadAirsimLevelInstance(UWorld* WorldContextObject, FString LevelName, FVector Location, FRotator Rotation, bool& bOutSuccess); + +private: + static int32 LevelInstanceId; +}; \ No newline at end of file diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.cpp b/Unreal/Plugins/AirSim/Source/NedTransform.cpp index 112e54d355..384f74a513 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.cpp +++ b/Unreal/Plugins/AirSim/Source/NedTransform.cpp @@ -66,6 +66,14 @@ FVector NedTransform::fromLocalNed(const NedTransform::Vector3r& position) const { return NedTransform::toFVector(position, world_to_meters_, true) + local_ned_offset_; } +FVector NedTransform::fromRelativeNed(const NedTransform::Vector3r& position) const +{ + return NedTransform::toFVector(position, world_to_meters_, true); +} +FTransform NedTransform::fromRelativeNed(const Pose& pose) const +{ + return FTransform(fromNed(pose.orientation), fromRelativeNed(pose.position)); +} FVector NedTransform::fromGlobalNed(const NedTransform::Vector3r& position) const { return NedTransform::toFVector(position, world_to_meters_, true) + global_transform_.GetLocation(); diff --git a/Unreal/Plugins/AirSim/Source/NedTransform.h b/Unreal/Plugins/AirSim/Source/NedTransform.h index 1c3ebf644e..717b1aaa14 100644 --- a/Unreal/Plugins/AirSim/Source/NedTransform.h +++ b/Unreal/Plugins/AirSim/Source/NedTransform.h @@ -42,6 +42,8 @@ class AIRSIM_API NedTransform FVector fromGlobalNed(const Vector3r& position) const; FQuat fromNed(const Quaternionr& q) const; float fromNed(float length) const; + FVector fromRelativeNed(const Vector3r& position) const; + FTransform fromRelativeNed(const Pose& pose) const; FTransform fromLocalNed(const Pose& pose) const; FTransform fromGlobalNed(const Pose& pose) const; diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp index 03ef6c3369..735298788c 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.cpp +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.cpp @@ -244,8 +244,12 @@ void APIPCamera::setCameraTypeEnabled(ImageType type, bool enabled) enableCaptureComponent(type, enabled); } -void APIPCamera::setCameraOrientation(const FRotator& rotator) +void APIPCamera::setCameraPose(const FTransform& pose) { + FVector position = pose.GetLocation(); + this->SetActorRelativeLocation(pose.GetLocation()); + + FRotator rotator = pose.GetRotation().Rotator(); if (gimbal_stabilization_ > 0) { gimbald_rotator_.Pitch = rotator.Pitch; gimbald_rotator_.Roll = rotator.Roll; @@ -286,12 +290,27 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera const auto& noise_setting = camera_setting.noise_settings.at(image_type); if (image_type >= 0) { //scene capture components - if (image_type==0 || image_type==5 || image_type==6 || image_type==7) - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], false, - image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform); - else - updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], true, - image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform); + switch (Utils::toEnum(image_type)) { + case ImageType::Scene: + case ImageType::Infrared: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + false, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + false); + break; + + case ImageType::Segmentation: + case ImageType::SurfaceNormals: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + true); + break; + + default: + updateCaptureComponentSetting(captures_[image_type], render_targets_[image_type], + true, image_type_to_pixel_format_map_[image_type], capture_setting, ned_transform, + false); + break; + } setNoiseMaterial(image_type, captures_[image_type], captures_[image_type]->PostProcessSettings, noise_setting); } @@ -304,7 +323,8 @@ void APIPCamera::setupCameraFromSettings(const APIPCamera::CameraSetting& camera } void APIPCamera::updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, - bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform) + bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform, + bool force_linear_gamma) { if (auto_format) { @@ -312,7 +332,7 @@ void APIPCamera::updateCaptureComponentSetting(USceneCaptureComponent2D* capture } else { - render_target->InitCustomFormat(setting.width, setting.height, pixel_format, false); + render_target->InitCustomFormat(setting.width, setting.height, pixel_format, force_linear_gamma); } if (!std::isnan(setting.target_gamma)) diff --git a/Unreal/Plugins/AirSim/Source/PIPCamera.h b/Unreal/Plugins/AirSim/Source/PIPCamera.h index 99cfb54da0..a1465163b1 100644 --- a/Unreal/Plugins/AirSim/Source/PIPCamera.h +++ b/Unreal/Plugins/AirSim/Source/PIPCamera.h @@ -41,7 +41,7 @@ class AIRSIM_API APIPCamera : public ACameraActor void setCameraTypeEnabled(ImageType type, bool enabled); bool getCameraTypeEnabled(ImageType type) const; void setupCameraFromSettings(const APIPCamera::CameraSetting& camera_setting, const NedTransform& ned_transform); - void setCameraOrientation(const FRotator& rotator); + void setCameraPose(const FTransform& pose); void setCameraFoV(float fov_degrees); msr::airlib::ProjectionMatrix getProjectionMatrix(const APIPCamera::ImageType image_type) const; @@ -75,8 +75,9 @@ class AIRSIM_API APIPCamera : public ACameraActor static unsigned int imageTypeCount(); void enableCaptureComponent(const ImageType type, bool is_enabled); - static void updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, - bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform); + static void updateCaptureComponentSetting(USceneCaptureComponent2D* capture, UTextureRenderTarget2D* render_target, + bool auto_format, const EPixelFormat& pixel_format, const CaptureSetting& setting, const NedTransform& ned_transform, + bool force_linear_gamma); void setNoiseMaterial(int image_type, UObject* outer, FPostProcessSettings& obj, const NoiseSetting& settings); static void updateCameraPostProcessingSetting(FPostProcessSettings& obj, const CaptureSetting& setting); static void updateCameraSetting(UCameraComponent* camera, const CaptureSetting& setting, const NedTransform& ned_transform); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp index 0f3ab3946b..9da9d1d61d 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.cpp @@ -409,12 +409,12 @@ msr::airlib::CameraInfo PawnSimApi::getCameraInfo(const std::string& camera_name return camera_info; } -void PawnSimApi::setCameraOrientation(const std::string& camera_name, const msr::airlib::Quaternionr& orientation) +void PawnSimApi::setCameraPose(const std::string& camera_name, const msr::airlib::Pose& pose) { - UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, orientation]() { + UAirBlueprintLib::RunCommandOnGameThread([this, camera_name, pose]() { APIPCamera* camera = getCamera(camera_name); - FQuat quat = ned_transform_.fromNed(orientation); - camera->setCameraOrientation(quat.Rotator()); + FTransform pose_unreal = ned_transform_.fromRelativeNed(pose); + camera->setCameraPose(pose_unreal); }, true); } @@ -514,8 +514,8 @@ void PawnSimApi::updateKinematics(float dt) next_kinematics.twist.angular = msr::airlib::VectorMath::toAngularVelocity( kinematics_->getPose().orientation, next_kinematics.pose.orientation, dt); - next_kinematics.accelerations.linear = (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt; - next_kinematics.accelerations.angular = (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt; + next_kinematics.accelerations.linear = dt > 0 ? (next_kinematics.twist.linear - kinematics_->getTwist().linear) / dt : next_kinematics.accelerations.linear; + next_kinematics.accelerations.angular = dt > 0 ? (next_kinematics.twist.angular - kinematics_->getTwist().angular) / dt : next_kinematics.accelerations.angular; kinematics_->setState(next_kinematics); kinematics_->update(); diff --git a/Unreal/Plugins/AirSim/Source/PawnSimApi.h b/Unreal/Plugins/AirSim/Source/PawnSimApi.h index c67922abfd..2148ae16cb 100644 --- a/Unreal/Plugins/AirSim/Source/PawnSimApi.h +++ b/Unreal/Plugins/AirSim/Source/PawnSimApi.h @@ -77,7 +77,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase { virtual Pose getPose() const override; virtual void setPose(const Pose& pose, bool ignore_collision) override; virtual msr::airlib::CameraInfo getCameraInfo(const std::string& camera_name) const override; - virtual void setCameraOrientation(const std::string& camera_name, const Quaternionr& orientation) override; + virtual void setCameraPose(const std::string& camera_name, const Pose& pose) override; virtual void setCameraFoV(const std::string& camera_name, float fov_degrees) override; virtual CollisionInfo getCollisionInfo() const override; virtual int getRemoteControlID() const override; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp index 77a56954d5..74f353f9ef 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.cpp @@ -23,6 +23,8 @@ void ASimHUD::BeginPlay() try { UAirBlueprintLib::OnBeginPlay(); + loadLevel(); + initializeSettings(); setUnrealEngineSettings(); createSimMode(); @@ -282,6 +284,13 @@ std::string ASimHUD::getSimModeFromUser() return "Car"; } +void ASimHUD::loadLevel() +{ + if (AirSimSettings::singleton().level_name != "") + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString(AirSimSettings::singleton().level_name.c_str()));}, true); + else + UAirBlueprintLib::RunCommandOnGameThread([&]() {UAirBlueprintLib::loadLevel(this->GetWorld(), FString("Blocks"));}, true); +} void ASimHUD::createSimMode() { std::string simmode_name = AirSimSettings::singleton().simmode_name; @@ -357,9 +366,10 @@ bool ASimHUD::getSettingsText(std::string& settingsText) readSettingsTextFromFile(FString(msr::airlib::Settings::Settings::getUserDirectoryFullPath("settings.json").c_str()), settingsText)); } -// Attempts to parse the settings text from the command line +// Attempts to parse the settings file path or the settings text from the command line // Looks for the flag "--settings". If it exists, settingsText will be set to the value. -// Example: AirSim.exe -s '{"foo" : "bar"}' -> settingsText will be set to {"foo": "bar"} +// Example (Path): AirSim.exe --settings "C:\path\to\settings.json" +// Example (Text): AirSim.exe -s '{"foo" : "bar"}' -> settingsText will be set to {"foo": "bar"} // Returns true if the argument is present, false otherwise. bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) { @@ -372,6 +382,11 @@ bool ASimHUD::getSettingsTextFromCommandLine(std::string& settingsText) FString commandLineArgsFString = FString(commandLineArgs); int idx = commandLineArgsFString.Find(TEXT("-settings")); FString settingsJsonFString = commandLineArgsFString.RightChop(idx + 10); + + if (readSettingsTextFromFile(settingsJsonFString.TrimQuotes(), settingsText)) { + return true; + } + if (FParse::QuotedString(*settingsJsonFString, settingsTextFString)) { settingsText = std::string(TCHAR_TO_UTF8(*settingsTextFString)); found = true; diff --git a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h index 06bde50870..fab7fdf82f 100644 --- a/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h +++ b/Unreal/Plugins/AirSim/Source/SimHUD/SimHUD.h @@ -59,6 +59,7 @@ class AIRSIM_API ASimHUD : public AHUD void createSimMode(); void initializeSettings(); void setUnrealEngineSettings(); + void loadLevel(); void createMainWidget(); const std::vector& getSubWindowSettings() const; std::vector& getSubWindowSettings(); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h new file mode 100644 index 0000000000..93bd97e793 --- /dev/null +++ b/Unreal/Plugins/AirSim/Source/SimMode/LoadingScreenWidget.h @@ -0,0 +1,15 @@ +// Fill out your copyright notice in the Description page of Project Settings. + +#pragma once + +#include "CoreMinimal.h" +#include "Blueprint/UserWidget.h" +#include "Runtime/UMG/Public/Components/Image.h" +#include "LoadingScreenWidget.generated.h" + + +UCLASS() +class AIRSIM_API ULoadingScreenWidget : public UUserWidget +{ + GENERATED_BODY() +}; diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 1aa75cc494..74f923c658 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp @@ -16,6 +16,7 @@ #include "SimJoyStick/SimJoyStick.h" #include "common/EarthCelestial.hpp" #include "sensors/lidar/LidarSimple.hpp" +#include "sensors/distance/DistanceSimple.hpp" #include "Weather/WeatherLib.h" @@ -26,9 +27,17 @@ //it to AirLib and directly implement WorldSimApiBase interface #include "WorldSimApi.h" +ASimModeBase *ASimModeBase::SIMMODE = nullptr; + +ASimModeBase* ASimModeBase::getSimMode() +{ + return SIMMODE; +} ASimModeBase::ASimModeBase() { + SIMMODE = this; + static ConstructorHelpers::FClassFinder external_camera_class(TEXT("Blueprint'/AirSim/Blueprints/BP_PIPCamera'")); external_camera_class_ = external_camera_class.Succeeded() ? external_camera_class.Class : nullptr; static ConstructorHelpers::FClassFinder camera_director_class(TEXT("Blueprint'/AirSim/Blueprints/BP_CameraDirector'")); @@ -47,6 +56,30 @@ ASimModeBase::ASimModeBase() static ConstructorHelpers::FClassFinder sky_sphere_class(TEXT("Blueprint'/Engine/EngineSky/BP_Sky_Sphere'")); sky_sphere_class_ = sky_sphere_class.Succeeded() ? sky_sphere_class.Class : nullptr; + + static ConstructorHelpers::FClassFinder loading_screen_class_find(TEXT("WidgetBlueprint'/AirSim/Blueprints/BP_LoadingScreenWidget'")); + if (loading_screen_class_find.Succeeded()) + { + auto loading_screen_class = loading_screen_class_find.Class; + loading_screen_widget_ = CreateWidget(this->GetWorld(), loading_screen_class); + + } + else + loading_screen_widget_ = nullptr; + +} + +void ASimModeBase::toggleLoadingScreen(bool is_visible) +{ + if (loading_screen_widget_ == nullptr) + return; + else { + + if (is_visible) + loading_screen_widget_->SetVisibility(ESlateVisibility::Visible); + else + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); + } } void ASimModeBase::BeginPlay() @@ -90,6 +123,9 @@ void ASimModeBase::BeginPlay() UWeatherLib::initWeather(World, spawned_actors_); //UWeatherLib::showWeatherMenu(World); } + + loading_screen_widget_->AddToViewport(); + loading_screen_widget_->SetVisibility(ESlateVisibility::Hidden); } const NedTransform& ASimModeBase::getGlobalNedTransform() @@ -110,7 +146,6 @@ void ASimModeBase::checkVehicleReady() UAirBlueprintLib::LogMessage("Tip: check connection info in settings.json", "", LogDebugLevel::Informational); } } - } } @@ -119,8 +154,7 @@ void ASimModeBase::setStencilIDs() UAirBlueprintLib::SetMeshNamingMethod(getSettings().segmentation_setting.mesh_naming_method); if (getSettings().segmentation_setting.init_method == - AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { - + AirSimSettings::SegmentationSetting::InitMethodType::CommonObjectsRandomIDs) { UAirBlueprintLib::InitializeMeshStencilIDs(!getSettings().segmentation_setting.override_existing); } //else don't init @@ -271,6 +305,8 @@ void ASimModeBase::Tick(float DeltaSeconds) drawLidarDebugPoints(); + drawDistanceSensorDebugPoints(); + Super::Tick(DeltaSeconds); } @@ -505,8 +541,8 @@ void ASimModeBase::setupVehiclesAndCamera() //compute initial pose FVector spawn_position = uu_origin.GetLocation(); - msr::airlib::Vector3r settings_position = vehicle_setting.position; - if (!msr::airlib::VectorMath::hasNan(settings_position)) + Vector3r settings_position = vehicle_setting.position; + if (!VectorMath::hasNan(settings_position)) spawn_position = getGlobalNedTransform().fromGlobalNed(settings_position); FRotator spawn_rotation = toFRotator(vehicle_setting.rotation, uu_origin.Rotator()); @@ -637,13 +673,12 @@ void ASimModeBase::drawLidarDebugPoints() msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); if (api != nullptr) { - - msr::airlib::uint count_lidars = api->getSensors().size(msr::airlib::SensorBase::SensorType::Lidar); + msr::airlib::uint count_lidars = api->getSensors().size(SensorType::Lidar); for (msr::airlib::uint i = 0; i < count_lidars; i++) { // TODO: Is it incorrect to assume LidarSimple here? const msr::airlib::LidarSimple* lidar = - static_cast(api->getSensors().getByType(msr::airlib::SensorBase::SensorType::Lidar, i)); + static_cast(api->getSensors().getByType(SensorType::Lidar, i)); if (lidar != nullptr && lidar->getParams().draw_debug_points) { lidar_draw_debug_points_ = true; @@ -653,7 +688,7 @@ void ASimModeBase::drawLidarDebugPoints() return; for (int j = 0; j < lidar_data.point_cloud.size(); j = j + 3) { - msr::airlib::Vector3r point(lidar_data.point_cloud[j], lidar_data.point_cloud[j + 1], lidar_data.point_cloud[j + 2]); + Vector3r point(lidar_data.point_cloud[j], lidar_data.point_cloud[j + 1], lidar_data.point_cloud[j + 2]); FVector uu_point; @@ -662,7 +697,7 @@ void ASimModeBase::drawLidarDebugPoints() } else if (lidar->getParams().data_frame == AirSimSettings::kSensorLocalFrame) { - msr::airlib::Vector3r point_w = msr::airlib::VectorMath::transformToWorldFrame(point, lidar_data.pose, true); + Vector3r point_w = VectorMath::transformToWorldFrame(point, lidar_data.pose, true); uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point_w); } else @@ -683,4 +718,51 @@ void ASimModeBase::drawLidarDebugPoints() } lidar_checks_done_ = true; -} \ No newline at end of file +} + +// Draw debug-point on main viewport for Distance sensor hit +void ASimModeBase::drawDistanceSensorDebugPoints() +{ + if (getApiProvider() == nullptr) + return; + + for (auto& sim_api : getApiProvider()->getVehicleSimApis()) { + PawnSimApi* pawn_sim_api = static_cast(sim_api); + std::string vehicle_name = pawn_sim_api->getVehicleName(); + + msr::airlib::VehicleApiBase* api = getApiProvider()->getVehicleApi(vehicle_name); + + if (api != nullptr) { + msr::airlib::uint count_distance_sensors = api->getSensors().size(SensorType::Distance); + Pose vehicle_pose = pawn_sim_api->getGroundTruthKinematics()->pose; + + for (msr::airlib::uint i=0; i(api->getSensors().getByType(SensorType::Distance, i)); + + if (distance_sensor != nullptr && distance_sensor->getParams().draw_debug_points) { + msr::airlib::DistanceSensorData distance_sensor_data = distance_sensor->getOutput(); + + // Find position of point hit + // Similar to UnrealDistanceSensor.cpp#L19 + // order of Pose addition is important here because it also adds quaternions which is not commutative! + Pose distance_sensor_pose = distance_sensor_data.relative_pose + vehicle_pose; + Vector3r start = distance_sensor_pose.position; + Vector3r point = start + VectorMath::rotateVector(VectorMath::front(), + distance_sensor_pose.orientation, true) * distance_sensor_data.distance; + + FVector uu_point = pawn_sim_api->getNedTransform().fromLocalNed(point); + + DrawDebugPoint( + this->GetWorld(), + uu_point, + 10, // size + FColor::Green, + false, // persistent (never goes away) + 0.03 // LifeTime: point leaves a trail on moving object + ); + } + } + } + } +} diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index bf116989dd..1b0b678ffe 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -14,9 +14,11 @@ #include "api/ApiProvider.hpp" #include "PawnSimApi.h" #include "common/StateReporterWrapper.hpp" +#include "LoadingScreenWidget.h" #include "SimModeBase.generated.h" +DECLARE_DYNAMIC_MULTICAST_DELEGATE(FLevelLoaded); UCLASS() class AIRSIM_API ASimModeBase : public AActor @@ -25,6 +27,9 @@ class AIRSIM_API ASimModeBase : public AActor GENERATED_BODY() + UPROPERTY(BlueprintAssignable, BlueprintCallable) + FLevelLoaded OnLevelLoaded; + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "Refs") ACameraDirector* CameraDirector; @@ -34,7 +39,15 @@ class AIRSIM_API ASimModeBase : public AActor UFUNCTION(BlueprintCallable, Category = "Recording") bool toggleRecording(); -public: + UFUNCTION(BlueprintPure, Category = "Airsim | get stuff") + static ASimModeBase* getSimMode(); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + void toggleLoadingScreen(bool is_visible); + + UFUNCTION(BlueprintCallable, Category = "Airsim | get stuff") + virtual void reset(); + // Sets default values for this actor's properties ASimModeBase(); virtual void BeginPlay() override; @@ -42,7 +55,6 @@ class AIRSIM_API ASimModeBase : public AActor virtual void Tick( float DeltaSeconds ) override; //additional overridable methods - virtual void reset(); virtual std::string getDebugReport(); virtual ECameraDirectorMode getInitialViewMode() const; @@ -110,17 +122,23 @@ class AIRSIM_API ASimModeBase : public AActor UPROPERTY() UClass* pip_camera_class; UPROPERTY() UParticleSystem* collision_display_template; + private: typedef common_utils::Utils Utils; typedef msr::airlib::ClockFactory ClockFactory; typedef msr::airlib::TTimePoint TTimePoint; typedef msr::airlib::TTimeDelta TTimeDelta; + typedef msr::airlib::SensorBase::SensorType SensorType; + typedef msr::airlib::Vector3r Vector3r; + typedef msr::airlib::Pose Pose; + typedef msr::airlib::VectorMath VectorMath; private: //assets loaded in constructor UPROPERTY() UClass* external_camera_class_; UPROPERTY() UClass* camera_director_class_; UPROPERTY() UClass* sky_sphere_class_; + UPROPERTY() ULoadingScreenWidget* loading_screen_widget_; UPROPERTY() AActor* sky_sphere_; @@ -147,7 +165,7 @@ class AIRSIM_API ASimModeBase : public AActor bool lidar_checks_done_ = false; bool lidar_draw_debug_points_ = false; - + static ASimModeBase* SIMMODE; private: void setStencilIDs(); void initializeTimeOfDay(); @@ -156,4 +174,5 @@ class AIRSIM_API ASimModeBase : public AActor void setupPhysicsLoopPeriod(); void showClockStats(); void drawLidarDebugPoints(); + void drawDistanceSensorDebugPoints(); }; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index c9780e63a0..7180fba12a 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -1,13 +1,137 @@ #include "WorldSimApi.h" +#include "common/common_utils/Utils.hpp" #include "AirBlueprintLib.h" #include "TextureShuffleActor.h" #include "common/common_utils/Utils.hpp" #include "Weather/WeatherLib.h" #include "DrawDebugHelpers.h" +#include +#include WorldSimApi::WorldSimApi(ASimModeBase* simmode) - : simmode_(simmode) + : simmode_(simmode) {} + +bool WorldSimApi::loadLevel(const std::string& level_name) +{ + bool success; + using namespace std::chrono_literals; + + // Add loading screen to viewport + simmode_->toggleLoadingScreen(true); + std::this_thread::sleep_for(0.1s); + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + + this->current_level_ = UAirBlueprintLib::loadLevel(this->simmode_->GetWorld(), FString(level_name.c_str())); + }, true); + + if (this->current_level_) + { + success = true; + std::this_thread::sleep_for(1s); + spawnPlayer(); + } + else + success = false; + + //Remove Loading screen from viewport + UAirBlueprintLib::RunCommandOnGameThread([this, level_name]() { + this->simmode_->OnLevelLoaded.Broadcast(); + }, true); + this->simmode_->toggleLoadingScreen(false); + + return success; +} + +void WorldSimApi::spawnPlayer() +{ + using namespace std::chrono_literals; + UE_LOG(LogTemp, Log, TEXT("spawning player")); + bool success{ false }; + + UAirBlueprintLib::RunCommandOnGameThread([&]() { + success = UAirBlueprintLib::spawnPlayer(this->simmode_->GetWorld()); + }, true); + + if (!success) + { + UE_LOG(LogTemp, Error, TEXT("Could not find valid PlayerStart Position")); + } + else + { + std::this_thread::sleep_for(1s); + simmode_->reset(); + } +} + +bool WorldSimApi::destroyObject(const std::string& object_name) +{ + bool result{ false }; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) + { + actor->Destroy(); + result = actor->IsPendingKill(); + } + }, true); + return result; +} + +std::string WorldSimApi::spawnObject(std::string& object_name, const std::string& load_object, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) +{ + // Create struct for Location and Rotation of actor in Unreal + FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose); + bool found_object; + UAirBlueprintLib::RunCommandOnGameThread([this, load_object, &object_name, &actor_transform, &found_object, &scale]() { + // Find mesh in /Game and /AirSim asset registry. When more plugins are added this function will have to change + UStaticMesh* LoadObject = dynamic_cast(UAirBlueprintLib::GetMeshFromRegistry(load_object)); + if (LoadObject) + { + 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()); + this->createNewActor(new_actor_spawn_params, actor_transform, scale, LoadObject); + found_object = true; + } + else + { + found_object = false; + } + }, true); + + if (!found_object) + { + throw std::invalid_argument( + "There were no objects with name " + load_object + " found in the Registry"); + } + return object_name; +} + +void 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); // new + UStaticMeshComponent* ObjectComponent = NewObject(NewActor); + ObjectComponent->SetStaticMesh(static_mesh); + ObjectComponent->SetRelativeLocation(FVector(0, 0, 0)); + ObjectComponent->SetWorldScale3D(FVector(scale[0], scale[1], scale[2])); + ObjectComponent->SetHiddenInGame(false, true); + ObjectComponent->RegisterComponent(); + NewActor->SetRootComponent(ObjectComponent); + NewActor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics); } bool WorldSimApi::isPaused() const @@ -80,6 +204,18 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con result = actor ? simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation())) : Pose::nanPose(); }, true); + + return result; +} + +WorldSimApi::Vector3r WorldSimApi::getObjectScale(const std::string& object_name) const +{ + Vector3r result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + result = actor ? Vector3r(actor->GetActorScale().X, actor->GetActorScale().Y, actor->GetActorScale().Z) + : Vector3r::Zero(); + }, true); return result; } @@ -101,6 +237,21 @@ bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimAp return result; } +bool WorldSimApi::setObjectScale(const std::string& object_name, const Vector3r& scale) +{ + bool result; + UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &scale, &result]() { + AActor* actor = UAirBlueprintLib::FindActor(simmode_, FString(object_name.c_str())); + if (actor) { + actor->SetActorScale3D(FVector(scale[0], scale[1], scale[2])); + result = true; + } + else + result = false; + }, true); + return result; +} + void WorldSimApi::enableWeather(bool enable) { UWeatherLib::setWeatherEnabled(simmode_->GetWorld(), enable); @@ -116,41 +267,42 @@ void WorldSimApi::setWeatherParameter(WeatherParameter param, float val) std::unique_ptr> WorldSimApi::swapTextures(const std::string& tag, int tex_id, int component_id, int material_id) { - auto swappedObjectNames = std::make_unique>(); - UAirBlueprintLib::RunCommandOnGameThread([this, &tag, tex_id, component_id, material_id, &swappedObjectNames]() { - //Split the tag string into individual tags. - TArray splitTags; - FString notSplit = FString(tag.c_str()); - FString next = ""; - while (notSplit.Split(",", &next, ¬Split)) - { - next.TrimStartInline(); - splitTags.Add(next); - } - notSplit.TrimStartInline(); - splitTags.Add(notSplit); - - //Texture swap on actors that have all of those tags. - TArray shuffleables; - UAirBlueprintLib::FindAllActor(simmode_, shuffleables); - for (auto *shuffler : shuffleables) - { - bool invalidChoice = false; - for (auto required_tag : splitTags) - { - invalidChoice |= !shuffler->ActorHasTag(FName(*required_tag)); - if (invalidChoice) - break; - } - - if (invalidChoice) - continue; - dynamic_cast(shuffler)->SwapTexture(tex_id, component_id, material_id); - swappedObjectNames->push_back(TCHAR_TO_UTF8(*shuffler->GetName())); - } - }, true); - return swappedObjectNames; + auto swappedObjectNames = std::make_unique>(); + UAirBlueprintLib::RunCommandOnGameThread([this, &tag, tex_id, component_id, material_id, &swappedObjectNames]() { + //Split the tag string into individual tags. + TArray splitTags; + FString notSplit = FString(tag.c_str()); + FString next = ""; + while (notSplit.Split(",", &next, ¬Split)) + { + next.TrimStartInline(); + splitTags.Add(next); + } + notSplit.TrimStartInline(); + splitTags.Add(notSplit); + + //Texture swap on actors that have all of those tags. + TArray shuffleables; + UAirBlueprintLib::FindAllActor(simmode_, shuffleables); + for (auto *shuffler : shuffleables) + { + bool invalidChoice = false; + for (auto required_tag : splitTags) + { + invalidChoice |= !shuffler->ActorHasTag(FName(*required_tag)); + if (invalidChoice) + break; + } + + if (invalidChoice) + continue; + dynamic_cast(shuffler)->SwapTexture(tex_id, component_id, material_id); + swappedObjectNames->push_back(TCHAR_TO_UTF8(*shuffler->GetName())); + } + }, true); + return swappedObjectNames; } + //----------- Plotting APIs ----------/ void WorldSimApi::simFlushPersistentMarkers() { @@ -237,4 +389,20 @@ std::vector WorldSimApi::getMesh responses = UAirBlueprintLib::GetStaticMeshComponents(); }, true); return responses; -} \ No newline at end of file +} + +// Recording APIs +void WorldSimApi::startRecording() +{ + simmode_->startRecording(); +} + +void WorldSimApi::stopRecording() +{ + simmode_->stopRecording(); +} + +bool WorldSimApi::isRecording() const +{ + return simmode_->isRecording(); +} diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index c5c7cf55bf..7b647af934 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -4,17 +4,25 @@ #include "common/CommonStructs.hpp" #include "api/WorldSimApiBase.hpp" #include "SimMode/SimModeBase.h" +#include "Components/StaticMeshComponent.h" +#include "Runtime/Engine/Classes/Engine/StaticMesh.h" +#include "Engine/LevelStreamingDynamic.h" #include class WorldSimApi : public msr::airlib::WorldSimApiBase { public: typedef msr::airlib::Pose Pose; typedef msr::airlib::Vector3r Vector3r; - typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; + typedef msr::airlib::MeshPositionVertexBuffersResponse MeshPositionVertexBuffersResponse; WorldSimApi(ASimModeBase* simmode); virtual ~WorldSimApi() = default; + virtual bool loadLevel(const std::string& level_name) override; + + virtual std::string spawnObject(std::string& object_name, const std::string& load_name, const WorldSimApi::Pose& pose, const WorldSimApi::Vector3r& scale) override; + virtual bool destroyObject(const std::string& object_name) override; + virtual bool isPaused() const override; virtual void reset() override; virtual void pause(bool is_paused) override; @@ -32,10 +40,12 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void printLogMessage(const std::string& message, const std::string& message_param = "", unsigned char severity = 0) override; - virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) override; + virtual std::unique_ptr> swapTextures(const std::string& tag, int tex_id = 0, int component_id = 0, int material_id = 0) 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; + virtual Vector3r getObjectScale(const std::string& object_name) const override; + virtual bool setObjectScale(const std::string& object_name, const Vector3r& scale) override; //----------- Plotting APIs ----------/ virtual void simFlushPersistentMarkers() override; @@ -46,8 +56,18 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase { virtual void simPlotStrings(const std::vector& strings, const std::vector& positions, float scale, const std::vector& color_rgba, float duration) override; virtual void simPlotTransforms(const std::vector& poses, float scale, float thickness, float duration, bool is_persistent) override; virtual void simPlotTransformsWithNames(const std::vector& poses, const std::vector& names, float tf_scale, float tf_thickness, float text_scale, const std::vector& text_color_rgba, float duration) override; - virtual std::vector getMeshPositionVertexBuffers() const override; + virtual std::vector getMeshPositionVertexBuffers() const override; + + // Recording APIs + virtual void startRecording() override; + virtual void stopRecording() override; + virtual bool isRecording() const override; + +private: + void createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh); + void spawnPlayer(); private: ASimModeBase* simmode_; + ULevelStreamingDynamic* current_level_; }; diff --git a/azure/.gitignore b/azure/.gitignore new file mode 100644 index 0000000000..f0f1cccf7f --- /dev/null +++ b/azure/.gitignore @@ -0,0 +1 @@ +!.vscode \ No newline at end of file diff --git a/azure/.vscode/extensions.json b/azure/.vscode/extensions.json new file mode 100644 index 0000000000..88a39fc08c --- /dev/null +++ b/azure/.vscode/extensions.json @@ -0,0 +1,7 @@ +{ + "recommendations": [ + "ms-python.python", + "ms-azuretools.vscode-docker", + "ms-vscode.powershell" + ] +} diff --git a/azure/.vscode/launch.json b/azure/.vscode/launch.json new file mode 100644 index 0000000000..17e15f27ec --- /dev/null +++ b/azure/.vscode/launch.json @@ -0,0 +1,15 @@ +{ + // Use IntelliSense to learn about possible attributes. + // Hover to view descriptions of existing attributes. + // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 + "version": "0.2.0", + "configurations": [ + { + "name": "Python: Current File", + "type": "python", + "request": "launch", + "program": "${file}", + "console": "integratedTerminal" + } + ] +} \ No newline at end of file diff --git a/azure/.vscode/tasks.json b/azure/.vscode/tasks.json new file mode 100644 index 0000000000..f4294d52df --- /dev/null +++ b/azure/.vscode/tasks.json @@ -0,0 +1,14 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Start AirSim", + "type": "shell", + "options": {"shell": {"executable": "powershell.exe"}}, + "command": ".\\start-airsim.ps1" + + } + ] +} \ No newline at end of file diff --git a/azure/app/multirotor.py b/azure/app/multirotor.py new file mode 100644 index 0000000000..8fe53c6420 --- /dev/null +++ b/azure/app/multirotor.py @@ -0,0 +1,42 @@ +import airsim +import pprint + +def print_state(client): + state = client.getMultirotorState() + s = pprint.pformat(state) + print("state: %s" % s) + + imu_data = client.getImuData() + s = pprint.pformat(imu_data) + print("imu_data: %s" % s) + + barometer_data = client.getBarometerData() + s = pprint.pformat(barometer_data) + print("barometer_data: %s" % s) + + magnetometer_data = client.getMagnetometerData() + s = pprint.pformat(magnetometer_data) + print("magnetometer_data: %s" % s) + + gps_data = client.getGpsData() + s = pprint.pformat(gps_data) + print("gps_data: %s" % s) + +# connect to the AirSim simulator +client = airsim.MultirotorClient() +client.confirmConnection() +client.enableApiControl(True) +client.armDisarm(True) + +print_state(client) +print('Takeoff') +client.takeoffAsync().join() + +while True: + print_state(client) + print('Go to (-10, 10, -10) at 5 m/s') + client.moveToPositionAsync(-10, 10, -10, 5).join() + client.hoverAsync().join() + print_state(client) + print('Go to (0, 10, 0) at 5 m/s') + client.moveToPositionAsync(0, 10, 0, 5).join() diff --git a/azure/app/requirements.txt b/azure/app/requirements.txt new file mode 100644 index 0000000000..ca180bf3ec --- /dev/null +++ b/azure/app/requirements.txt @@ -0,0 +1 @@ +airsim \ No newline at end of file diff --git a/azure/app/settings.json b/azure/app/settings.json new file mode 100644 index 0000000000..44b5a20009 --- /dev/null +++ b/azure/app/settings.json @@ -0,0 +1,19 @@ +{ + "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings_json.md", + "SettingsVersion": 1.2, + "SimMode": "Multirotor", + "ClockSpeed": 1.0, + "Vehicles": { + "SimpleFlight": { + "VehicleType": "SimpleFlight", + "DefaultVehicleState": "Armed", + "EnableCollisionPassthrogh": false, + "EnableCollisions": true, + "AllowAPIAlways": true, + "RC": { + "RemoteControlID": 0, + "AllowAPIWhenDisconnected": false + } + } + } +} \ No newline at end of file diff --git a/azure/azure-env-creation/configure-vm.ps1 b/azure/azure-env-creation/configure-vm.ps1 new file mode 100644 index 0000000000..3910298c07 --- /dev/null +++ b/azure/azure-env-creation/configure-vm.ps1 @@ -0,0 +1,37 @@ +$airSimInstallPath = "C:\AirSim\" +$airSimBinaryZipUrl = "https://github.com/microsoft/AirSim/releases/download/v1.3.1-windows/Blocks.zip" +$airSimBinaryZipFilename = "Blocks.zip" +$airSimBinaryPath = $airSimInstallPath + "blocks\blocks\binaries\win64\blocks.exe" +$airSimBinaryName = "Blocks" + +$webClient = new-object System.Net.WebClient + +# Install the OpenSSH Client +Add-WindowsCapability -Online -Name OpenSSH.Client~~~~0.0.1.0 +# Install the OpenSSH Server +Add-WindowsCapability -Online -Name OpenSSH.Server~~~~0.0.1.0 +# Enable service +Start-Service sshd +Set-Service -Name sshd -StartupType 'Automatic' + +#Install Chocolatey +Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iwr https://chocolatey.org/install.ps1 -UseBasicParsing | iex +# Bypass confirmation in scripts. +choco feature enable --name="'allowGlobalConfirmation'" +choco install python --version=3.8.2 +choco install git +# Run time c++ +choco install vcredist-all +choco install directx + +#Create new folder & set as default directory +New-Item -ItemType directory -Path $airSimInstallPath +cd $airSimInstallPath + +# Get AirSim +$webClient.DownloadFile($airSimBinaryZipUrl, $airSimInstallPath + $airSimBinaryZipFilename) +# Unzip AirSim +Expand-Archive $airSimBinaryZipFilename + +# Firewall rule for AirSim +New-NetFirewallRule -DisplayName $airSimBinaryName -Direction Inbound -Program $airSimBinaryPath -Action Allow diff --git a/azure/azure-env-creation/vm-arm-template.json b/azure/azure-env-creation/vm-arm-template.json new file mode 100644 index 0000000000..e87babba6d --- /dev/null +++ b/azure/azure-env-creation/vm-arm-template.json @@ -0,0 +1,250 @@ +{ + "$schema": "https://schema.management.azure.com/schemas/2019-04-01/deploymentTemplate.json#", + "contentVersion": "1.0.0.0", + "parameters": { + "VmName": { + "type": "string", + "metadata": { + "description": "Name for the VM can be whatever you want, but can't be changed once the VM is created." + } + }, + "AdminUsername": { + "type": "string", + "metadata": { + "description": "Admin user name for the VM." + }, + "defaultValue": "AzureUser" + }, + "AdminPassword": { + "type": "securestring", + "metadata": { + "description": "Admin user password for the VM." + } + }, + "VmSize": { + "type": "string", + "metadata": { + "description": "Desired Size of the VM (NV-series installs NVIDIA GPU drivers in WDDM mode for graphical display by default)." + }, + "defaultValue": "Standard_NV6", + "allowedValues": [ + "Standard_NV6_Promo", + "Standard_NV6", + "Standard_NV12", + "Standard_NV24" + ] + }, + "ScriptLocation": { + "type": "string", + "metadata": { + "description": "Location of the setup script" + }, + "defaultValue": "https://raw.githubusercontent.com/microsoft/airsim/master/azure/azure-env-creation" + }, + "ScriptFileName": { + "type": "string", + "metadata": { + "description": "Name of the setup script" + }, + "defaultValue": "configure-vm.ps1" + } + + }, + "variables": { + "NetworkInterfaceCardName": "[concat(parameters('VmName'),'-nic')]", + "PublicIPAddressName": "[concat(parameters('VmName'),'-ip')]", + "NetworkSecurityGroupName": "[concat(parameters('VmName'),'-nsg')]", + "VirtualNetworkName": "[concat(parameters('VmName'),'-vnet')]" + }, + "resources": [ + { + "type": "Microsoft.Network/networkSecurityGroups", + "apiVersion": "2019-12-01", + "name": "[variables('NetworkSecurityGroupName')]", + "location": "[resourceGroup().location]", + "properties": { + "securityRules": [ + { + "name": "RDP", + "properties": { + "protocol": "Tcp", + "sourcePortRange": "*", + "destinationPortRange": "3389", + "sourceAddressPrefix": "*", + "destinationAddressPrefix": "*", + "access": "Allow", + "priority": 300, + "direction": "Inbound" + } + }, + { + "name": "SSH", + "properties": { + "protocol": "Tcp", + "sourcePortRange": "*", + "destinationPortRange": "22", + "sourceAddressPrefix": "*", + "destinationAddressPrefix": "*", + "access": "Allow", + "priority": 320, + "direction": "Inbound" + } + } + + ] + } + }, + { + "type": "Microsoft.Network/virtualNetworks/subnets", + "apiVersion": "2019-12-01", + "name": "[concat(variables('VirtualNetworkName'), '/default')]", + "dependsOn": [ + "[resourceId('Microsoft.Network/virtualNetworks', variables('VirtualNetworkName'))]" + ], + "properties": { + "addressPrefix": "10.0.0.0/24" + } + }, + { + "type": "Microsoft.Network/virtualNetworks", + "apiVersion": "2019-12-01", + "name": "[variables('VirtualNetworkName')]", + "location": "[resourceGroup().location]", + "properties": { + "addressSpace": { + "addressPrefixes": [ + "10.0.0.0/24" + ] + }, + "subnets": [ + { + "name": "default", + "properties": { + "addressPrefix": "10.0.0.0/24" + } + } + ] + } + }, + { + "type": "Microsoft.Network/publicIPAddresses", + "apiVersion": "2019-12-01", + "name": "[variables('PublicIPAddressName')]", + "location": "[resourceGroup().location]", + "properties": { + "publicIPAllocationMethod": "Dynamic" + } + }, + { + "type": "Microsoft.Network/networkInterfaces", + "apiVersion": "2019-12-01", + "name": "[variables('NetworkInterfaceCardName')]", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Network/publicIPAddresses', variables('PublicIPAddressName'))]", + "[resourceId('Microsoft.Network/networkSecurityGroups', variables('NetworkSecurityGroupName'))]", + "[resourceId('Microsoft.Network/virtualNetworks/subnets', variables('VirtualNetworkName'), 'default')]" + ], + "properties": { + "ipConfigurations": [ + { + "name": "ipconfig1", + "properties": { + "publicIPAddress": { + "id": "[resourceId('Microsoft.Network/publicIPAddresses', variables('PublicIPAddressName'))]" + }, + "subnet": { + "id": "[resourceId('Microsoft.Network/virtualNetworks/subnets', variables('VirtualNetworkName'), 'default')]" + } + } + } + ], + "networkSecurityGroup": { + "id": "[resourceId('Microsoft.Network/networkSecurityGroups', variables('NetworkSecurityGroupName'))]" + } + } + }, + { + "type": "Microsoft.Compute/virtualMachines", + "apiVersion": "2019-07-01", + "name": "[parameters('VmName')]", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Network/networkInterfaces', variables('NetworkInterfaceCardName'))]" + ], + "properties": { + "hardwareProfile": { + "vmSize": "[parameters('vmSize')]" + }, + "storageProfile": { + "imageReference": { + "publisher": "MicrosoftWindowsDesktop", + "offer": "Windows-10", + "sku": "rs5-pro", + "version": "latest" + }, + "osDisk": { + "osType": "Windows", + "name": "[concat(parameters('VmName'), '_OsDisk')]", + "createOption": "FromImage", + "caching": "ReadWrite" + } + }, + "osProfile": { + "computerName": "[parameters('VmName')]", + "adminUsername": "[parameters('AdminUsername')]", + "adminPassword": "[parameters('AdminPassword')]" + }, + "networkProfile": { + "networkInterfaces": [ + { + "id": "[resourceId('Microsoft.Network/networkInterfaces', variables('NetworkInterfaceCardName'))]" + } + ] + } + } + }, + { + "name": "[concat(parameters('VmName'),'/GPUDrivers')]", + "type": "Microsoft.Compute/virtualMachines/extensions", + "location": "[resourceGroup().location]", + "apiVersion": "2019-07-01", + "dependsOn": [ + "[resourceId('Microsoft.Compute/virtualMachines/', parameters('VmName'))]" + ], + "properties": { + "publisher": "Microsoft.HpcCompute", + "type": "NvidiaGpuDriverWindows", + "typeHandlerVersion": "1.3", + "autoUpgradeMinorVersion": true, + "settings": { + } + } + }, + { + "name": "[concat(parameters('VmName'),'/SetupScript')]", + "apiVersion": "2019-07-01", + "type": "Microsoft.Compute/virtualMachines/extensions", + "location": "[resourceGroup().location]", + "dependsOn": [ + "[resourceId('Microsoft.Compute/virtualMachines/extensions', parameters('VmName'), 'GPUDrivers')]" + ], + "properties": { + "publisher": "Microsoft.Compute", + "type": "CustomScriptExtension", + "typeHandlerVersion": "1.10", + "autoUpgradeMinorVersion": true, + "settings": { + "fileUris": [ + "[concat(parameters('ScriptLocation'), '/' , parameters('ScriptFileName'))]" + ], + "commandToExecute": "[concat('powershell.exe -ExecutionPolicy bypass -File ./', parameters('ScriptFileName'))]" + }, + "protectedSettings": { + } + } + } + ], + "outputs": { + } +} \ No newline at end of file diff --git a/azure/docker/Dockerfile b/azure/docker/Dockerfile new file mode 100644 index 0000000000..27766f87f7 --- /dev/null +++ b/azure/docker/Dockerfile @@ -0,0 +1,63 @@ +FROM nvidia/cudagl:9.0-devel + +RUN apt-get update +RUN apt-get install \ + sudo \ + libglu1-mesa-dev \ + xdg-user-dirs \ + pulseaudio \ + sudo \ + x11-xserver-utils \ + unzip \ + wget \ + software-properties-common \ + -y --no-install-recommends + +RUN apt-add-repository ppa:deadsnakes/ppa +RUN apt-get update +RUN apt-get install -y \ + python3.6 \ + python3-pip + +RUN python3.6 -m pip install --upgrade pip + +RUN python3.6 -m pip install setuptools wheel + +RUN adduser --force-badname --disabled-password --gecos '' --shell /bin/bash airsim_user && \ + echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers && \ + adduser airsim_user sudo && \ + adduser airsim_user audio && \ + adduser airsim_user video + +USER airsim_user +WORKDIR /home/airsim_user + +# Change the following values to use a different AirSim binary +# Also change the AIRSIM_EXECUTABLE variable in docker-entrypoint.sh +ENV AIRSIM_BINARY_ZIP_URL=https://github.com/microsoft/AirSim/releases/download/v1.3.1-linux/Blocks.zip +ENV AIRSIM_BINARY_ZIP_FILENAME=Blocks.zip + +ENV SDL_VIDEODRIVER_VALUE=offscreen +ENV SDL_HINT_CUDA_DEVICE=0 + +# Download and unzip the AirSim binary +RUN wget -c $AIRSIM_BINARY_ZIP_URL +RUN unzip $AIRSIM_BINARY_ZIP_FILENAME +RUN rm $AIRSIM_BINARY_ZIP_FILENAME + +# Add the Python app to the image +ADD ./app /home/airsim_user/app + +WORKDIR /home/airsim_user +RUN mkdir -p /home/airsim_user/Documents/AirSim +ADD ./app/settings.json /home/airsim_user/Documents/AirSim +ADD ./docker/docker-entrypoint.sh /home/airsim_user/docker-entrypoint.sh + +RUN sudo chown -R airsim_user /home/airsim_user + +WORKDIR /home/airsim_user/app + +# Install Python requirements +RUN python3.6 -m pip install -r requirements.txt + +ENTRYPOINT /home/airsim_user/docker-entrypoint.sh \ No newline at end of file diff --git a/azure/docker/docker-entrypoint.sh b/azure/docker/docker-entrypoint.sh new file mode 100644 index 0000000000..436736918c --- /dev/null +++ b/azure/docker/docker-entrypoint.sh @@ -0,0 +1,11 @@ +#!/bin/bash +AIRSIM_EXECUTABLE=/home/airsim_user/Blocks/Blocks.sh + +echo Starting AirSim binary... +$AIRSIM_EXECUTABLE & + +echo Waiting 10 seconds before starting app... +sleep 10 + +echo Starting Python app +python3.6 /home/airsim_user/app/multirotor.py \ No newline at end of file diff --git a/azure/start-airsim.ps1 b/azure/start-airsim.ps1 new file mode 100644 index 0000000000..6f52061501 --- /dev/null +++ b/azure/start-airsim.ps1 @@ -0,0 +1,23 @@ +# Script parameters +$airSimExecutable = "c:\AirSim\Blocks\blocks.exe" +$airSimProcessName = "Blocks" + +# Ensure proper path +$env:Path = + [System.Environment]::GetEnvironmentVariable("Path","Machine") + ";" + + [System.Environment]::GetEnvironmentVariable("Path","User") + +# Install python app requirements +pip3 install -r .\app\requirements.txt + +# Overwrite AirSim configuration +New-Item -ItemType Directory -Force -Path $env:USERPROFILE\Documents\AirSim\ +copy .\app\settings.json $env:USERPROFILE\Documents\AirSim\ + +# Kill previous AirSim instance +Stop-Process -Name $airSimProcessName -Force -ErrorAction SilentlyContinue +sleep 2 + +# Start new AirSim instance +Start-Process -NoNewWindow -FilePath $airSimExecutable -ArgumentList "-RenderOffScreen" +echo "Starting the AirSim environment has completed." diff --git a/build.cmd b/build.cmd index 2b7e4b6632..43a28c7e45 100644 --- a/build.cmd +++ b/build.cmd @@ -56,7 +56,7 @@ IF NOT EXIST external\rpclib\rpclib-2.2.1 ( ECHO Downloading rpclib ECHO ***************************************************************************************** @echo on - powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/rpclib/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" + powershell -command "& { [Net.ServicePointManager]::SecurityProtocol = [Net.SecurityProtocolType]::Tls12; iwr https://github.com/madratman/rpclib/archive/v2.2.1.zip -OutFile external\rpclib.zip }" @echo off REM //remove any previous versions @@ -157,15 +157,15 @@ IF NOT EXIST AirLib\deps\eigen3 goto :buildfailed REM //---------- now we have all dependencies to compile AirSim.sln which will also compile MavLinkCom ---------- if "%buildMode%" == "--Debug" ( -msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) else if "%buildMode%" == "--Release" ( -msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) else ( -msbuild /p:Platform=x64 /p:Configuration=Debug AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Debug AirSim.sln if ERRORLEVEL 1 goto :buildfailed -msbuild /p:Platform=x64 /p:Configuration=Release AirSim.sln +msbuild -maxcpucount:12 /p:Platform=x64 /p:Configuration=Release AirSim.sln if ERRORLEVEL 1 goto :buildfailed ) diff --git a/build.sh b/build.sh index 952b739657..8c4c5fe8b9 100755 --- a/build.sh +++ b/build.sh @@ -7,6 +7,22 @@ pushd "$SCRIPT_DIR" >/dev/null set -e set -x +debug=false + +# Parse command line arguments +while [[ $# -gt 0 ]] +do +key="$1" + +case $key in +--debug) + debug=true + shift # past argument + ;; +esac + +done + function version_less_than_equal_to() { test "$(printf '%s\n' "$@" | sort -V | head -n 1)" = "$1"; } # check for rpclib @@ -28,7 +44,11 @@ else fi # variable for build output -build_dir=build_debug +if $debug; then + build_dir=build_debug +else + build_dir=build_release +fi if [ "$(uname)" == "Darwin" ]; then export CC=/usr/local/opt/llvm@8/bin/clang export CXX=/usr/local/opt/llvm@8/bin/clang++ @@ -53,13 +73,23 @@ if [[ -d "./cmake/CMakeFiles" ]]; then rm -rf "./cmake/CMakeFiles" fi +folder_name="" + if [[ ! -d $build_dir ]]; then mkdir -p $build_dir pushd $build_dir >/dev/null - "$CMAKE" ../cmake -DCMAKE_BUILD_TYPE=Debug \ - || (popd && rm -r $build_dir && exit 1) - popd >/dev/null + if $debug; then + folder_name="Debug" + "$CMAKE" ../cmake -DCMAKE_BUILD_TYPE=Debug \ + || (popd && rm -r $build_dir && exit 1) + popd >/dev/null + else + folder_name="Release" + "$CMAKE" ../cmake -DCMAKE_BUILD_TYPE=Release \ + || (popd && rm -r $build_dir && exit 1) + popd >/dev/null + fi fi pushd $build_dir >/dev/null @@ -69,7 +99,7 @@ pushd $build_dir >/dev/null make -j`nproc` popd >/dev/null -mkdir -p AirLib/lib/x64/Debug +mkdir -p AirLib/lib/x64/$folder_name mkdir -p AirLib/deps/rpclib/lib mkdir -p AirLib/deps/MavLinkCom/lib cp $build_dir/output/lib/libAirLib.a AirLib/lib @@ -77,10 +107,11 @@ cp $build_dir/output/lib/libMavLinkCom.a AirLib/deps/MavLinkCom/lib cp $build_dir/output/lib/librpc.a AirLib/deps/rpclib/lib/librpc.a # Update AirLib/lib, AirLib/deps, Plugins folders with new binaries -rsync -a --delete $build_dir/output/lib/ AirLib/lib/x64/Debug +rsync -a --delete $build_dir/output/lib/ AirLib/lib/x64/$folder_name rsync -a --delete external/rpclib/rpclib-2.2.1/include AirLib/deps/rpclib rsync -a --delete MavLinkCom/include AirLib/deps/MavLinkCom rsync -a --delete AirLib Unreal/Plugins/AirSim/Source +rm -rf Unreal/Plugins/AirSim/Source/AirLib/src # Update Blocks project Unreal/Environments/Blocks/clean.sh diff --git a/check_cmake.bat b/check_cmake.bat index e998c6cbdc..13097b257a 100644 --- a/check_cmake.bat +++ b/check_cmake.bat @@ -3,7 +3,7 @@ REM //---------- set up variable ---------- setlocal set ROOT_DIR=%~dp0 -set cmake_minversion_minmaj=" 3. 9" +set cmake_minversion_minmaj=" 3. 14" set "cmake_version= . " @@ -38,12 +38,12 @@ exit /b 0 :download_install set /p choice="Press any key to download and install cmake (make sure to add it in path in install options)" -IF NOT EXIST %temp%\cmake-3.10.2-win64-x64.msi ( +IF NOT EXIST %temp%\cmake-3.14.7-win64-x64.msi ( @echo on - powershell -command "& { iwr https://cmake.org/files/v3.10/cmake-3.10.2-win64-x64.msi -OutFile %temp%\cmake-3.10.2-win64-x64.msi }" + powershell -command "& { iwr https://cmake.org/files/v3.14/cmake-3.14.7-win64-x64.msi -OutFile %temp%\cmake-3.14.7-win64-x64.msi }" @echo off ) -msiexec.exe /i "%temp%\cmake-3.10.2-win64-x64.msi" +msiexec.exe /i "%temp%\cmake-3.14.7-win64-x64.msi" exit /b 1 \ No newline at end of file diff --git a/cmake/cmake-modules/CommonSetup.cmake b/cmake/cmake-modules/CommonSetup.cmake index bca7a8f9b6..22241de7d7 100644 --- a/cmake/cmake-modules/CommonSetup.cmake +++ b/cmake/cmake-modules/CommonSetup.cmake @@ -59,7 +59,7 @@ macro(CommonSetup) ${RPC_LIB_DEFINES} ${CMAKE_CXX_FLAGS}") if (${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") - set(CMAKE_CXX_FLAGS "-stdlib=libc++ -Wno-documentation ${CMAKE_CXX_FLAGS}") + set(CMAKE_CXX_FLAGS "-stdlib=libc++ -Wno-documentation -Wno-unknown-warning-option ${CMAKE_CXX_FLAGS}") set(CXX_EXP_LIB "-lc++fs -ferror-limit=10") else() set(CXX_EXP_LIB "-lstdc++fs -fmax-errors=10 -Wnoexcept -Wstrict-null-sentinel") @@ -68,6 +68,9 @@ macro(CommonSetup) set(BUILD_PLATFORM "x64") set(CMAKE_POSITION_INDEPENDENT_CODE ON) + if (CMAKE_BUILD_TYPE MATCHES Release) + set(CMAKE_CXX_FLAGS "-O3 ${CMAKE_CXX_FLAGS}") + endif () ELSE() #windows cmake build is experimental diff --git a/docker/Dockerfile_binary b/docker/Dockerfile_binary index 12f1d9fe86..7c633bfbf5 100644 --- a/docker/Dockerfile_binary +++ b/docker/Dockerfile_binary @@ -1,4 +1,4 @@ -ARG BASE_IMAGE=nvidia/cudagl:10.0-devel-ubuntu16.04 +ARG BASE_IMAGE=nvidia/cudagl:10.0-devel-ubuntu18.04 FROM $BASE_IMAGE RUN apt-get update diff --git a/docker/build_airsim_image.py b/docker/build_airsim_image.py index 6614a7f3fe..e5130927dc 100644 --- a/docker/build_airsim_image.py +++ b/docker/build_airsim_image.py @@ -22,7 +22,7 @@ def build_docker_image(args): else: dockerfile = 'Dockerfile_binary' if not args.base_image: - args.base_image = "nvidia/cudagl:10.0-devel-ubuntu16.04" + args.base_image = "nvidia/cudagl:10.0-devel-ubuntu18.04" target_image_tag = args.base_image.split(":")[1] # take tag from base image if not args.target_image: args.target_image = 'airsim_binary' + ':' + target_image_tag diff --git a/docs/airsim_ros_pkgs.md b/docs/airsim_ros_pkgs.md index cdbf65b35a..160b51825f 100644 --- a/docs/airsim_ros_pkgs.md +++ b/docs/airsim_ros_pkgs.md @@ -9,11 +9,11 @@ Verify version by `gcc --version` - Ubuntu 16.04 * Install [ROS kinetic](https://wiki.ros.org/kinetic/Installation/Ubuntu) - * Install mavros packages: `sudo apt-get install ros-kinetic-mavros*` + * Install tf2 sensor and mavros packages: `sudo apt-get install ros-kinetic-tf2-sensor-msgs ros-kinetic-mavros*` - Ubuntu 18.04 * Install [ROS melodic](https://wiki.ros.org/melodic/Installation/Ubuntu) - * Install mavros packages: `sudo apt-get install ros-melodic-mavros*` + * Install tf2 sensor and mavros packages: `sudo apt-get install ros-melodic-tf2-sensor-msgs ros-melodic-mavros*` - Install [catkin_tools](https://catkin-tools.readthedocs.io/en/latest/installing.html) `sudo apt-get install python-catkin-tools` or @@ -60,7 +60,7 @@ GPS coordinates corresponding to global NED frame. This is set in the airsim's [ This the current GPS coordinates of the drone in airsim. - `/airsim_node/VEHICLE_NAME/odom_local_ned` [nav_msgs/Odometry](https://docs.ros.org/api/nav_msgs/html/msg/Odometry.html) -Odometry in NED frame wrt take-off point. +Odometry in NED frame (default name: odom_local_ned, launch name and frame type are configurable) wrt take-off point. - `/airsim_node/VEHICLE_NAME/CAMERA_NAME/IMAGE_TYPE/camera_info` [sensor_msgs/CameraInfo](https://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html) @@ -69,6 +69,19 @@ Odometry in NED frame wrt take-off point. - `/tf` [tf2_msgs/TFMessage](https://docs.ros.org/api/tf2_msgs/html/msg/TFMessage.html) +- `/airsim_node/VEHICLE_NAME/altimeter/SENSOR_NAME` [airsim_ros_pkgs::Altimeter] This the current altimeter reading for altitude, pressure, and QNH (https://en.wikipedia.org/wiki/QNH) + +- `/airsim_node/VEHICLE_NAME/imu/SENSOR_NAME` [sensor_msgs::Imu] (http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) + IMU sensor data + +- `/airsim_node/VEHICLE_NAME/magnetometer/SENSOR_NAME` [sensor_msgs::MagneticField] (http://docs.ros.org/api/sensor_msgs/html/msg/MagneticField.html) + Meausrement of magnetic field vector/compass + +- `/airsim_node/VEHICLE_NAME/distance/SENSOR_NAME` [sensor_msgs::Range] (http://docs.ros.org/api/sensor_msgs/html/msg/Range.html) + Meausrement of distance from an active ranger, such as infrared or IR + +- `/airsim_node/VEHICLE_NAME/lidar/SENSOR_NAME` [sensor_msgs::PointCloud2] (http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud2.html) + LIDAR pointcloud #### Subscribers: - `/airsim_node/vel_cmd_body_frame` [airsim_ros_pkgs/VelCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/VelCmd.msg) @@ -83,6 +96,9 @@ Odometry in NED frame wrt take-off point. - `/gimbal_angle_quat_cmd` [airsim_ros_pkgs/GimbalAngleQuatCmd](https://github.com/microsoft/AirSim/tree/master/ros/src/airsim_ros_pkgs/msg/GimbalAngleQuatCmd.msg) Gimbal set point in quaternion. +- `/airsim_node/VEHICLE_NAME/car_cmd` [airsim_ros_pkgs/CarControls] + Throttle, brake, steering and gear selections for control. Both automatic and manual transmission control possible, see the car_joy.py script for use. + #### Services: - `/airsim_node/VEHICLE_NAME/land` [airsim_ros_pkgs/Takeoff](https://docs.ros.org/api/std_srvs/html/srv/Empty.html) @@ -92,20 +108,40 @@ Odometry in NED frame wrt take-off point. Resets *all* drones #### Parameters: +- `/airsim_node/world_frame_id` [string] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: world_ned + Set to "world_enu" to switch to ENU frames automatically + +- `/airsim_node/odom_frame_id` [string] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: odom_local_ned + If you set world_frame_id to "world_enu", the default odom name will instead default to "odom_local_enu" + +- `/airsim_node/coordinate_system_enu` [boolean] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: false + If you set world_frame_id to "world_enu", this setting will instead default to true + - `/airsim_node/update_airsim_control_every_n_sec` [double] Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` - Default: 0.01 seconds. + Default: 0.01 seconds. Timer callback frequency for updating drone odom and state from airsim, and sending in control commands. The current RPClib interface to unreal engine maxes out at 50 Hz. - Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. + Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. - `/airsim_node/update_airsim_img_response_every_n_sec` [double] Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` - Default: 0.01 seconds. + Default: 0.01 seconds. Timer callback frequency for receiving images from all cameras in airsim. The speed will depend on number of images requested and their resolution. Timer callbacks in ROS run at maximum rate possible, so it's best to not touch this parameter. +- `/airsim_node/publish_clock` [double] + Set in: `$(airsim_ros_pkgs)/launch/airsim_node.launch` + Default: false + Will publish the ros /clock topic if set to true. + ### Simple PID Position Controller Node #### Parameters: diff --git a/docs/apis.md b/docs/apis.md index e3ed611d6f..a8807f74b4 100644 --- a/docs/apis.md +++ b/docs/apis.md @@ -179,6 +179,20 @@ Please note that `Roadwetness`, `RoadSnow` and `RoadLeaf` effects requires addin Please see [example code](https://github.com/Microsoft/AirSim/blob/master/PythonClient/computer_vision/weather.py) for more details. +### Recording APIs + +Recording APIs can be used to start recording data through APIs. Data to be recorded can be specified using [settings](settings.md#recording). To start recording, use - + +``` +client.startRecording() +``` + +Similarly, to stop recording, use `client.stopRecording()`. To check whether Recording is running, call `client.isRecording()`, returns a `bool`. + +This API works alongwith toggling Recording using R button, therefore if it's enabled using R key, `isRecording()` will return `True`, and recording can be stopped via API using `stopRecording()`. Similarly, recording started using API will be stopped if R key is pressed in Viewport. LogMessage will also appear in the top-left of the viewport if recording is started or stopped using API. + +Note that this will only save the data as specfied in the settings. For full freedom in storing data such as certain sensor information, or in a different format or layout, use the other APIs to fetch the data and save as desired. + ### Lidar APIs AirSim offers API to retrieve point cloud data from Lidar sensors on vehicles. You can set the number of channels, points per second, horizontal and vertical FOV, etc parameters in [settings.json](settings.md). diff --git a/docs/azure.md b/docs/azure.md new file mode 100644 index 0000000000..24ace28094 --- /dev/null +++ b/docs/azure.md @@ -0,0 +1,84 @@ +# AirSim Development Environment on Azure + +This document explains how to automate the creation of a development environment on Azure and code and debug a Python application connected to AirSim using Visual Studio Code + +## Automatically Deploy Your Azure VM +Use [this](https://github.com/microsoft/AirSim/blob/master/azure/azure-env-creation/vm-arm-template.json) template to create, deploy and configure an Azure VM to work with AirSim + +*Note: the VM deployment and configuration process may take 20+ minutes to complete* + + + + + +### Regarding the deployment of the Azure VM +- When using an Azure Trial account, the default vCPU quota may not be enough to allocate the required VM to run AirSim. If that's the case, you will see an error when trying to create the VM and will have to make a support request to increase the default quota. + +- To avoid charges for the Virtual Machine usage while not in use, remember to deallocate its resources from the [Azure Portal](https://portal.azure.com) or use the following command from the Azure CLI: +```bash +az vm deallocate --resource-group MyResourceGroup --name MyVMName +``` + +## Code and debug from Visual Studio Code and Remote SSH +- Install Visual Studio Code +- Install the *Remote - SSH* extension +- Press `F1` and run the `Remote - SSH: Connect to host...` command +- Add the recently create VM details. For instance, `AzureUser@11.22.33.44` +- Run the `Remote - SSH: Connect to host...` command again, and now select the newly added connection. +- Once connected, click on the `Clone Repository` button in Visual Studio Code, and either clone this repository in the remote VM and open *just the `azure` folder*, or create a brand new repository, clone it and copy the contents of the `azure` folder from this repository in it. It is important to open that directory so Visual Studio Code can use the specific `.vscode` directory for the scenario and not the general AirSim `.vscode` directory. It contains the recommended extensions to install, the task to start AirSim remotely and the launch configuration for the Python application. +- Install all the recommended extensions +- Press `F1` and select the `Tasks: Run Task` option. Then, select the `Start AirSim` task from Visual Studio Code to execute the `start-airsim.ps1` script from Visual Studio Code. +- Open the `multirotor.py` file inside the `app` directory +- Start debugging with Python +- When finished, remember to stop an deallocate the Azure VM to avoid extra charges + +## Code and debug from a local Visual Studio Code and connect to AirSim via forwarded ports + +*Note: this scenario, will be using two Visual Studio Code instances. +The first one will be used as a bridge to forward ports via SSH to the Azure VM and execute remote processes, and the second one will +be used for local Python development. +To be able to reach the VM from the local Python code, it is required to keep the `Remote - SSH` instance of Visual Studio Code opened, while working with the local Python environment on the second instance* + +- Open the first Visual Studio Code instance +- Follow the steps in the previous section to connect via `Remote - SSH` +- In the *Remote Explorer*, add the port `41451` as a forwarded port to localhost +- Either run the `Start AirSim` task on the Visual Studio Code with the remote session as explained in the previous scenario or manually start the AirSim binary in the VM +- Open a second Visual Studio Code instance, without disconnecting or closing the first one +- Either clone this repository locally and open *just the `azure` folder* in Visual Studio Code, or create a brand new repository, clone it and copy the contents of the `azure` folder from this repository in it. +- Run `pip install -r requirements.txt` inside the `app` directory +- Open the `multirotor.py` file inside the `app` directory +- Start debugging with Python +- When finished, remember to stop an deallocate the Azure VM to avoid extra charges + +## Running with Docker +Once both the AirSim environment and the Python application are ready, you can package everything as a Docker image. The sample project inside the `azure` directory is already prepared to run a prebuilt AirSim binary and Python code using Docker. + +This would be a perfect scenario when you want to run the simulation at scale. For instance, you could have several different configurations for the same simulation and execute them in a parallel, unattended way using a Docker image on Azure Container Services + +Since AirSim requires access to the host GPU, it is required to use a Docker runtime that supports it. For more information about running AirSim in Docker, click [here](https://github.com/microsoft/AirSim/blob/master/docs/docker_ubuntu.md). + +When using Azure Container Services to run this image, the only extra-requirement is to add GPU support to the Container Group where it will be deployed. + +It can use either public docker images from DockerHub or images deployed to a private Azure Container Registry + +### Building the docker image + +```bash +docker build -t / -f ./docker/Dockerfile .` +``` + +## Using a different AirSim binary + +To use a different AirSim binary, first check the official documentation on [How to Build AirSim on Windows](build_windows.md) and [How to Build AirSim on Linux](build_linux.md) if you also want to run it with Docker + +Once you have a zip file with the new AirSim environment (or prefer to use one from the [Official Releases](https://github.com/microsoft/AirSim/releases)), you need to modify some of the scripts in the `azure` directory of the repository to point to the new environment: +- In [`azure/azure-env-creation/configure-vm.ps1`](https://github.com/microsoft/AirSim/blob/master/azure/azure-env-creation/configure-vm.ps1), modify all the parameters starting with `$airSimBinary` with the new values +- In [`azure/start-airsim.ps1`](https://github.com/microsoft/AirSim/blob/master/azure/start-airsim.ps1), modify `$airSimExecutable` and `$airSimProcessName` with the new values + +If you are using the docker image, you also need a linux binary zip file and modify the following Docker-related files: +- In [`azure/docker/Dockerfile`](https://github.com/microsoft/AirSim/blob/master/azure/docker/Dockerfile), modify the `AIRSIM_BINARY_ZIP_URL` and `AIRSIM_BINARY_ZIP_FILENAME` ENV declarations with the new values +- In [`azure/docker/docker-entrypoint.sh`](https://github.com/microsoft/AirSim/blob/master/azure/docker/docker-entrypoint.sh), modify `AIRSIM_EXECUTABLE` with the new value + +## Maintaining this development environment + +Several components of this development environment (ARM templates, initialization scripts and VSCode tasks) directly depend on the current directory structures file names and repository locations. When planning to modify/fork any of those, make sure to check every script and template to make any required adjustment. diff --git a/docs/build_linux.md b/docs/build_linux.md index 37519a7daf..7c7bc7a192 100644 --- a/docs/build_linux.md +++ b/docs/build_linux.md @@ -19,14 +19,14 @@ Please see instructions [here](https://github.com/Microsoft/AirSim/blob/master/d - Clone Unreal in your favorite folder and build it (this may take a while!). **Note**: We only support Unreal >= 4.22 at present. We recommend using 4.24. - ```bash - # go to the folder where you clone GitHub projects - git clone -b 4.24 https://github.com/EpicGames/UnrealEngine.git - cd UnrealEngine - ./Setup.sh - ./GenerateProjectFiles.sh - make - ``` +```bash +# go to the folder where you clone GitHub projects +git clone -b 4.24 https://github.com/EpicGames/UnrealEngine.git +cd UnrealEngine +./Setup.sh +./GenerateProjectFiles.sh +make +``` #### macOS - Download Unreal Engine @@ -41,18 +41,19 @@ Click on the `Add Versions` which should show the option to download **Unreal 4. - Clone AirSim and build it: - ```bash - # go to the folder where you clone GitHub projects - git clone https://github.com/Microsoft/AirSim.git - cd AirSim - ``` +```bash +# go to the folder where you clone GitHub projects +git clone https://github.com/Microsoft/AirSim.git +cd AirSim +``` - By default AirSim recommends using clang 8 to build the binaries as those will be compatible with UE 4.24. The setup script will install the right version of cmake, llvm, and eigen. + By default AirSim uses clang 8 to build for compatibility with UE 4.24. The setup script will install the right version of cmake, llvm, and eigen. - ```bash - ./setup.sh - ./build.sh - ``` +```bash +./setup.sh +./build.sh +# use ./build.sh --debug to build in debug mode +``` ### Build Unreal Environment diff --git a/docs/docker_ubuntu.md b/docs/docker_ubuntu.md index 54bc01aced..d4aeec6dc1 100644 --- a/docs/docker_ubuntu.md +++ b/docs/docker_ubuntu.md @@ -7,16 +7,17 @@ We've two options for docker. You can either build an image for running [airsim #### Build the docker image - Below are the default arguments. - `--base_image`: This is image over which we'll install airsim. We've tested on Ubuntu 16.04 + CUDA 10.0. + `--base_image`: This is image over which we'll install airsim. We've tested on Ubuntu 18.04 with CUDA 10.0. You can specify any [NVIDIA cudagl](https://hub.docker.com/r/nvidia/cudagl/) at your own risk. `--target_image` is the desired name of your docker image. Defaults to `airsim_binary` with same tag as the base image - ``` - $ cd Airsim/docker; - $ python build_airsim_image.py \ - --base_image=nvidia/cudagl:10.0-devel-ubuntu16.04 \ - --target_image=airsim_binary:10.0-devel-ubuntu16.04 - ``` + +```bash +$ cd Airsim/docker; +$ python build_airsim_image.py \ + --base_image=nvidia/cudagl:10.0-devel-ubuntu18.04 \ + --target_image=airsim_binary:10.0-devel-ubuntu18.04 +``` - Verify you have an image by: `$ docker images | grep airsim` @@ -25,28 +26,32 @@ We've two options for docker. You can either build an image for running [airsim - Get [an unreal binary](https://github.com/Microsoft/AirSim/releases/tag/v1.2.0Linux) or package your own project in Ubuntu. Let's take the Blocks binary as an example. You can download it by running - ``` - $ cd Airsim/docker; - $ ./download_blocks_env_binary.sh - ``` + +```bash + $ cd Airsim/docker; + $ ./download_blocks_env_binary.sh +``` - Running an unreal binary inside a docker container The syntax is: - ``` - $ ./run_airsim_image_binary.sh DOCKER_IMAGE_NAME UNREAL_BINARY_SHELL_SCRIPT UNREAL_BINARY_ARGUMENTS -- headless - ``` - For blocks, you can do a `$ ./run_airsim_image_binary.sh airsim_binary:10.0-devel-ubuntu16.04 Blocks/Blocks.sh -windowed -ResX=1080 -ResY=720` - * `DOCKER_IMAGE_NAME`: Same as `target_image` parameter in previous step. By default, enter `airsim_binary:10.0-devel-ubuntu16.04` +```bash + $ ./run_airsim_image_binary.sh DOCKER_IMAGE_NAME UNREAL_BINARY_SHELL_SCRIPT UNREAL_BINARY_ARGUMENTS -- headless +``` + + For blocks, you can do a `$ ./run_airsim_image_binary.sh airsim_binary:10.0-devel-ubuntu18.04 Blocks/Blocks.sh -windowed -ResX=1080 -ResY=720` + + * `DOCKER_IMAGE_NAME`: Same as `target_image` parameter in previous step. By default, enter `airsim_binary:10.0-devel-ubuntu18.04` * `UNREAL_BINARY_SHELL_SCRIPT`: for Blocks enviroment, it will be `Blocks/Blocks.sh` * [`UNREAL_BINARY_ARGUMENTS`](https://docs.unrealengine.com/en-us/Programming/Basics/CommandLineArguments): For airsim, most relevant would be `-windowed`, `-ResX`, `-ResY`. Click on link to see all options. - * Running in Headless mode: - Suffix `-- headless` at the end: - ``` - $ ./run_airsim_image_binary.sh Blocks/Blocks.sh -- headless - ``` + * Running in Headless mode: + Suffix `-- headless` at the end: +```bash +$ ./run_airsim_image_binary.sh Blocks/Blocks.sh -- headless +``` + - [Specifying a `settings.json`](https://github.com/Microsoft/AirSim/blob/master/docs/docker_ubuntu.md#airsim_binary-docker-image) ## Source @@ -79,18 +84,22 @@ You can download it by running `--base_image`: This is image over which we'll install airsim. We've tested on `adamrehn/ue4-engine:4.19.2-cudagl10.0`. See [ue4-docker](https://adamrehn.com/docs/ue4-docker/building-images/available-container-images) for other versions. `--target_image` is the desired name of your docker image. Defaults to `airsim_source` with same tag as the base image - ``` - $ cd Airsim/docker; - $ python build_airsim_image.py \ - --source \ - ----base_image adamrehn/ue4-engine:4.19.2-cudagl10.0 \ - --target_image=airsim_source:4.19.2-cudagl10.0 - ``` + +```bash +$ cd Airsim/docker; +$ python build_airsim_image.py \ + --source \ + ----base_image adamrehn/ue4-engine:4.19.2-cudagl10.0 \ + --target_image=airsim_source:4.19.2-cudagl10.0 +``` + #### Running AirSim container * Run the airsim source image we built by: - ``` - ./run_airsim_image_source.sh airsim_source:4.19.2-cudagl10.0 - ``` + +```bash + ./run_airsim_image_source.sh airsim_source:4.19.2-cudagl10.0 +``` + Syntax is `./run_airsim_image_source.sh DOCKER_IMAGE_NAME -- headless` `-- headless`: suffix this to run in optional headless mode. @@ -103,52 +112,56 @@ You can download it by running #### [Misc] Packaging Unreal Environments in `airsim_source` containers * Let's take the Blocks environment as an example. In the following script, specify the full path to your unreal uproject file by `project` and the directory where you want the binaries to be placed by `archivedirectory` - ``` - $ /home/ue4/UnrealEngine/Engine/Build/BatchFiles/RunUAT.sh BuildCookRun -platform=Linux -clientconfig=Shipping -serverconfig=Shipping -noP4 -cook -allmaps -build -stage -prereqs -pak -archive \ - -archivedirectory=/home/ue4/Binaries/Blocks/ \ - -project=/home/ue4/AirSim/Unreal/Environments/Blocks/Blocks.uproject - ``` + +```bash +$ /home/ue4/UnrealEngine/Engine/Build/BatchFiles/RunUAT.sh BuildCookRun -platform=Linux -clientconfig=Shipping -serverconfig=Shipping -noP4 -cook -allmaps -build -stage -prereqs -pak -archive \ +-archivedirectory=/home/ue4/Binaries/Blocks/ \ +-project=/home/ue4/AirSim/Unreal/Environments/Blocks/Blocks.uproject +``` This would create a Blocks binary in `/home/ue4/Binaries/Blocks/`. You can test it by running `/home/ue4/Binaries/Blocks/LinuxNoEditor/Blocks.sh -windowed` ### Specifying an airsim settings.json - #### `airsim_binary` docker image: +#### `airsim_binary` docker image: - We're mapping the host machine's `PATH/TO/Airsim/docker/settings.json` to the docker container's `/home/airsim_user/Documents/AirSim/settings.json`. - Hence, we can load any settings file by simply modifying `PATH_TO_YOUR/settings.json` by modifying the following snippets in * [`run_airsim_image_binary.sh`](https://github.com/Microsoft/AirSim/blob/master/docker/run_airsim_image_binary.sh) - ``` - nvidia-docker run -it \ - -v $PATH_TO_YOUR/settings.json:/home/airsim_user/Documents/AirSim/settings.json \ - -v $UNREAL_BINARY_PATH:$UNREAL_BINARY_PATH \ - -e SDL_VIDEODRIVER=$SDL_VIDEODRIVER_VALUE \ - -e SDL_HINT_CUDA_DEVICE='0' \ - --net=host \ - --env="DISPLAY=$DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - -env="XAUTHORITY=$XAUTH" \ - --volume="$XAUTH:$XAUTH" \ - --runtime=nvidia \ - --rm \ - $DOCKER_IMAGE_NAME \ - /bin/bash -c "$UNREAL_BINARY_COMMAND" - ``` - #### `airsim_source` docker image: + +```bash +nvidia-docker run -it \ + -v $PATH_TO_YOUR/settings.json:/home/airsim_user/Documents/AirSim/settings.json \ + -v $UNREAL_BINARY_PATH:$UNREAL_BINARY_PATH \ + -e SDL_VIDEODRIVER=$SDL_VIDEODRIVER_VALUE \ + -e SDL_HINT_CUDA_DEVICE='0' \ + --net=host \ + --env="DISPLAY=$DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + -env="XAUTHORITY=$XAUTH" \ + --volume="$XAUTH:$XAUTH" \ + --runtime=nvidia \ + --rm \ + $DOCKER_IMAGE_NAME \ + /bin/bash -c "$UNREAL_BINARY_COMMAND" +``` + +#### `airsim_source` docker image: * We're mapping the host machine's `PATH/TO/Airsim/docker/settings.json` to the docker container's `/home/airsim_user/Documents/AirSim/settings.json`. * Hence, we can load any settings file by simply modifying `PATH_TO_YOUR/settings.json` by modifying the following snippets in [`run_airsim_image_source.sh`](https://github.com/Microsoft/AirSim/blob/master/docker/run_airsim_image_source.sh): - ``` - nvidia-docker run -it \ - -v $(pwd)/settings.json:/home/airsim_user/Documents/AirSim/settings.json \ - -e SDL_VIDEODRIVER=$SDL_VIDEODRIVER_VALUE \ - -e SDL_HINT_CUDA_DEVICE='0' \ - --net=host \ - --env="DISPLAY=$DISPLAY" \ - --env="QT_X11_NO_MITSHM=1" \ - --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ - -env="XAUTHORITY=$XAUTH" \ - --volume="$XAUTH:$XAUTH" \ - --runtime=nvidia \ - --rm \ - $DOCKER_IMAGE_NAME - ``` + +```bash + nvidia-docker run -it \ + -v $(pwd)/settings.json:/home/airsim_user/Documents/AirSim/settings.json \ + -e SDL_VIDEODRIVER=$SDL_VIDEODRIVER_VALUE \ + -e SDL_HINT_CUDA_DEVICE='0' \ + --net=host \ + --env="DISPLAY=$DISPLAY" \ + --env="QT_X11_NO_MITSHM=1" \ + --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ + -env="XAUTHORITY=$XAUTH" \ + --volume="$XAUTH:$XAUTH" \ + --runtime=nvidia \ + --rm \ + $DOCKER_IMAGE_NAME +``` diff --git a/docs/drone_survey.md b/docs/drone_survey.md new file mode 100644 index 0000000000..50988da1a7 --- /dev/null +++ b/docs/drone_survey.md @@ -0,0 +1,50 @@ +# Implementing a Drone Survey script + +Moved here from [https://github.com/microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script](https://github.com/microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script) + +Ever wanted to capture a bunch of top-down pictures of a certain location? Well, the Python API makes this really simple. See the [code available here](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/survey.py). + +![survey](images/survey.png) + +Let's assume we want the following variables: + +| Variable | Description | +| ------------- | ------------- | +| boxsize | The overall size of the square box to survey | +| stripewidth | How far apart to drive the swim lanes, this can depend on the type of camera lens, for example. | +| altitude | The height to fly the survey. | +| speed | The speed of the survey can depend on how fast your camera can snap shots. | + +So with these we can compute a square path box using this code: + +```python + path = [] + distance = 0 + while x < self.boxsize: + distance += self.boxsize + path.append(Vector3r(x, self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, self.boxsize, z)) + distance += self.boxsize + path.append(Vector3r(x, -self.boxsize, z)) + x += self.stripewidth + distance += self.stripewidth + path.append(Vector3r(x, -self.boxsize, z)) + distance += self.boxsize +``` +Assuming we start in the corner of the box, increment x by the stripe width, then fly the full y-dimension of `-boxsize` to `+boxsize`, so in this case, `boxsize` is half the size of the actual box we will be covering. + +Once we have this list of Vector3r objects, we can fly this path very simply with the following call: +```python +result = self.client.moveOnPath(path, self.velocity, trip_time, DrivetrainType.ForwardOnly, + YawMode(False,0), lookahead, 1) +``` + +We can compute an appropriate `trip_time` timeout by dividing the distance of the path and the speed we are flying. + +The `lookahead` needed here for smooth path interpolation can be computed from the velocity using `self.velocity + (self.velocity/2)`. The more lookahead, the smoother the turns. This is why you see in the screenshot that the ends of each swimland are smooth turns rather than a square box pattern. This can result in a smoother video from your camera also. + +That's it, pretty simple, eh? + +Now of course you can add a lot more intelligence to this, make it avoid known obstacles on your map, make it climb up and down a hillside so you can survey a slope, etc. Lots of fun to be had. diff --git a/docs/flight_controller.md b/docs/flight_controller.md index 303473aafc..8b72333641 100644 --- a/docs/flight_controller.md +++ b/docs/flight_controller.md @@ -16,5 +16,6 @@ In "software-in-loop" simulation (SITL or SIL) mode the firmware runs in your co AirSim has built-in flight controller called [simple_flight](simple_flight.md) and it is used by default. You don't need to do anything to use or configure it. AirSim also supports [PX4](px4_setup.md) as another flight controller for advanced users. In the future, we also plan to support [ROSFlight](https://rosflight.org/) and [Hackflight](https://github.com/simondlevy/hackflight). - ## Using AirSim Without Flight Controller +## Using AirSim Without Flight Controller + Yes, now it's possible to use AirSim without flight controller. Please see the [instructions here](image_apis.md) for how to use so-called "Computer Vision" mode. If you don't need vehicle dynamics, we highly recommend using this mode. diff --git a/docs/image_apis.md b/docs/image_apis.md index c9ade83cb9..4076ae80bb 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -25,14 +25,13 @@ png_image = client.simGetImage("0", airsim.ImageType.Scene) int getOneImage() { - using namespace std; using namespace msr::airlib; - //for car use CarRpcLibClient - msr::airlib::MultirotorRpcLibClient client; + // for car use CarRpcLibClient + MultirotorRpcLibClient client; - vector png_image = client.simGetImage("0", VehicleCameraBase::ImageType::Scene); - //do something with images + std::vector png_image = client.simGetImage("0", VehicleCameraBase::ImageType::Scene); + // do something with images } ``` @@ -94,19 +93,18 @@ airsim.write_png(os.path.normpath(filename + '.png'), img_rgb) ```cpp int getStereoAndDepthImages() { - using namespace std; using namespace msr::airlib; typedef VehicleCameraBase::ImageRequest ImageRequest; typedef VehicleCameraBase::ImageResponse ImageResponse; typedef VehicleCameraBase::ImageType ImageType; - //for car use - //msr::airlib::CarRpcLibClient client; - msr::airlib::MultirotorRpcLibClient client; + // for car use + // CarRpcLibClient client; + MultirotorRpcLibClient client; - //get right, left and depth images. First two as png, second as float16. - vector request = { + // get right, left and depth images. First two as png, second as float16. + std::vector request = { //png format ImageRequest("0", ImageType::Scene), //uncompressed RGB array bytes @@ -115,8 +113,8 @@ int getStereoAndDepthImages() ImageRequest("1", ImageType::DepthPlanner, true) }; - const vector& response = client.simGetImages(request); - //do something with response which contains image data, pose, timestamp etc + const std::vector& response = client.simGetImages(request); + // do something with response which contains image data, pose, timestamp etc } ``` @@ -166,9 +164,10 @@ To move around the environment using APIs you can use `simSetVehiclePose` API. T ## Camera APIs The `simGetCameraInfo` returns the pose (in world frame, NED coordinates, SI units) and FOV (in degrees) for the specified camera. Please see [example usage](https://github.com/Microsoft/AirSim/tree/master/PythonClient//computer_vision/cv_mode.py). -The `simSetCameraOrientation` sets the orientation for the specified camera as quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch, you can use: +The `simSetCameraPose` sets the pose for the specified camera while taking an input pose as a combination of relative position and a quaternion in NED frame. The handy `airsim.to_quaternion()` function allows to convert pitch, roll, yaw to quaternion. For example, to set camera-0 to 15-degree pitch while maintaining the same position, you can use: ``` -client.simSetCameraOrientation(0, airsim.to_quaternion(0.261799, 0, 0)); #radians +camera_pose = airsim.Pose(airsim.Vector3r(0, 0, 0), airsim.to_quaternion(0.261799, 0, 0)) #RPY in radians +client.simSetCameraPose(0, camera_pose); ``` ### Gimbal @@ -221,7 +220,7 @@ When you specify `ImageType = DepthVis` in `ImageRequest`, you get an image that You normally want to retrieve disparity image as float (i.e. set `pixels_as_float = true` and specify `ImageType = DisparityNormalized` in `ImageRequest`) in which case each pixel is `(Xl - Xr)/Xmax`, which is thereby normalized to values between 0 to 1. ### Segmentation -When you specify `ImageType = Segmentation` in `ImageRequest`, you get an image that gives you ground truth segmentation of the scene. At the startup, AirSim assigns value 0 to 255 to each mesh available in environment. This value is than mapped to a specific color in [the pallet](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). The RGB values for each object ID can be found in [this file](seg_rgbs.txt). +When you specify `ImageType = Segmentation` in `ImageRequest`, you get an image that gives you ground truth segmentation of the scene. At the startup, AirSim assigns value 0 to 255 to each mesh available in environment. This value is then mapped to a specific color in [the pallet](https://github.com/Microsoft/AirSim/tree/master/Unreal//Plugins/AirSim/Content/HUDAssets/seg_color_pallet.png). The RGB values for each object ID can be found in [this file](seg_rgbs.txt). You can assign a specific value (limited to the range 0-255) to a specific mesh using APIs. For example, below Python code sets the object ID for the mesh called "Ground" to 20 in Blocks environment and hence changes its color in Segmentation view: @@ -258,7 +257,7 @@ A complete ready-to-run example can be found in [segmentation.py](https://github An object's ID can be set to -1 to make it not show up on the segmentation image. #### How to Find Mesh Names? -To get desired ground truth segmentation you will need to know the names of the meshes in your Unreal environment. To do this, you will need to open up Unreal Environment in Unreal Editor and then inspect the names of the meshes you are interested in using the World Outliner. For example, below we see the mesh names for he ground in Blocks environment in right panel in the editor: +To get desired ground truth segmentation you will need to know the names of the meshes in your Unreal environment. To do this, you will need to open up Unreal Environment in Unreal Editor and then inspect the names of the meshes you are interested in using the World Outliner. For example, below we see the mesh names for the ground in Blocks environment in right panel in the editor: ![record screenshot](images/unreal_editor_blocks.png) diff --git a/docs/images/depth.png b/docs/images/depth.png new file mode 100644 index 0000000000..1b1612b479 Binary files /dev/null and b/docs/images/depth.png differ diff --git a/docs/images/orbit.png b/docs/images/orbit.png new file mode 100644 index 0000000000..adf171d8d2 Binary files /dev/null and b/docs/images/orbit.png differ diff --git a/docs/images/point_cloud.png b/docs/images/point_cloud.png new file mode 100644 index 0000000000..2140120e32 Binary files /dev/null and b/docs/images/point_cloud.png differ diff --git a/docs/images/survey.png b/docs/images/survey.png new file mode 100644 index 0000000000..649f3c5bfd Binary files /dev/null and b/docs/images/survey.png differ diff --git a/docs/lidar.md b/docs/lidar.md index 0f6ad67025..6d07373f66 100644 --- a/docs/lidar.md +++ b/docs/lidar.md @@ -111,10 +111,8 @@ Use `getLidarData()` API to retrieve the Lidar data. * Segmentation: The segmentation of each lidar point's collided object ### Python Examples - -[drone_lidar.py](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/drone_lidar.py) - -[car_lidar.py](https://github.com/microsoft/AirSim/blob/master/PythonClient/car/car_lidar.py) +- [drone_lidar.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//multirotor) +- [car_lidar.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//car) ## Coming soon * Visualization of lidar data on client side. diff --git a/docs/orbit.md b/docs/orbit.md new file mode 100644 index 0000000000..78209fe7e7 --- /dev/null +++ b/docs/orbit.md @@ -0,0 +1,21 @@ +# An Orbit Trajectory + +Moved here from [https://github.com/microsoft/AirSim/wiki/An-Orbit-Trajectory](https://github.com/microsoft/AirSim/wiki/An-Orbit-Trajectory) + +Have you ever wanted to fly a nice smooth circular orbit? This can be handy for capturing 3D objects from all sides especially if you get multiple orbits at different altitudes. + +So the `PythonClient/multirotor` folder contains a script named [Orbit](https://github.com/microsoft/AirSim/blob/master/PythonClient/multirotor/orbit.py) that will do precisely that. + +See [demo video](https://youtu.be/RFG5CTQi3Us) + +The demo video was created by running this command line: + +```shell +python orbit.py --radius 10 --altitude 5 --speed 1 --center "0,1" --iterations 1 +``` + +This flies a 10 meter radius orbit around the center location at (startpos + radius * [0,1]), in other words, the center is located `radius` meters away in the direction of the provided center vector. It also keeps the front-facing camera on the drone always pointing at the center of the circle. If you watch the flight using LogViewer you will see a nice circular pattern get traced out on the GPS map: + +![image](images/orbit.png) + +The core of the algorithm is not that complicated. At each point on the circle, we look ahead by a small delta in degrees, called the `lookahead_angle`, where that angle is computed based on our desired velocity. We then find that lookahead point on the circle using sin/cosine and make that our "target point". Calculating the velocity then is easy, just subtract our current position from that point and feed that into the AirSim method `moveByVelocityZ`. diff --git a/docs/point_clouds.md b/docs/point_clouds.md new file mode 100644 index 0000000000..eb9e06cc19 --- /dev/null +++ b/docs/point_clouds.md @@ -0,0 +1,17 @@ +# Point Clouds + +Moved here from [https://github.com/microsoft/AirSim/wiki/Point-Clouds](https://github.com/microsoft/AirSim/wiki/Point-Clouds) + +A Python script [point_cloud.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/point_cloud.py) shows how to convert the depth image returned from AirSim into a point cloud. + +The following depth image was captured using the Modular Neighborhood environment: + +![depth](images/depth.png) + +And with the appropriate projection matrix, the OpenCV `reprojectImageTo3D` function can turn this into a point cloud. The following is the result, which is also available here: [https://skfb.ly/68r7y](https://skfb.ly/68r7y). + +![depth](images/point_cloud.png) + +[SketchFab](https://sketchfab.com) can upload the resulting file `cloud.asc` and render it for you. + +PS: you may notice the scene is reflected on the Y axis, so I may have a sign wrong in the projection matrix. An exercise for the reader :-) diff --git a/docs/px4_sitl.md b/docs/px4_sitl.md index 3eea3661b2..ae3702fdfb 100644 --- a/docs/px4_sitl.md +++ b/docs/px4_sitl.md @@ -3,7 +3,7 @@ The [PX4](http://dev.px4.io) software provides a "software-in-loop" simulation (SITL) version of their stack that runs in Linux. If you are on Windows then you must use the [Cygwin Toolchain](https://dev.px4.io/master/en/setup/dev_env_windows_cygwin.html) as the [Bash On Windows](https://dev.px4.io/master/en/setup/dev_env_windows_bash_on_win.html) toolchain no longer works for SITL. -**Note** that every time you stop the unreal app you have top restart the `px4` app. +**Note** that every time you stop the unreal app you have to restart the `px4` app. 1. From your bash terminal follow [these steps for Linux](http://dev.px4.io/starting-installing-linux.html) and follow **all** the instructions under `NuttX based hardware` to install prerequisites. We've also included out own copy of the [PX4 build instructions](px4_build.md) which is a bit more concise about what we need exactly. diff --git a/docs/remote_control.md b/docs/remote_control.md index 318b550c38..b50632e509 100644 --- a/docs/remote_control.md +++ b/docs/remote_control.md @@ -8,12 +8,13 @@ By default AirSim uses [simple_flight](simple_flight.md) as its flight controlle You can either use XBox controller or [FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html). Note that XBox 360 controller is not precise enough and is not recommended if you wanted more real world experience. See FAQ below if things are not working. - ### Other Devices - AirSim can detect large variety of devices however devices other than above *might* need extra configuration. In future we will add ability to set this config through settings.json. For now, if things are not working then you might want to try workarounds such as [x360ce](http://www.x360ce.com/) or change code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L50). +### Other Devices - ### Note on FrSky Taranis X9D Plus +AirSim can detect large variety of devices however devices other than above *might* need extra configuration. In future we will add ability to set this config through settings.json. For now, if things are not working then you might want to try workarounds such as [x360ce](http://www.x360ce.com/) or change code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L50). - [FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html) is real UAV remote control with an advantage that it has USB port so it can be directly connected to PC. You can [download AirSim config file](misc/AirSim_FrSkyTaranis.bin) and [follow this tutorial](https://www.youtube.com/watch?v=qe-13Gyb0sw) to import it in your RC. You should then see "sim" model in RC with all channels configured properly. +### Note on FrSky Taranis X9D Plus + +[FrSky Taranis X9D Plus](https://hobbyking.com/en_us/frsky-2-4ghz-accst-taranis-x9d-plus-and-x8r-combo-digital-telemetry-radio-system-mode-2.html) is real UAV remote control with an advantage that it has USB port so it can be directly connected to PC. You can [download AirSim config file](misc/AirSim_FrSkyTaranis.bin) and [follow this tutorial](https://www.youtube.com/watch?v=qe-13Gyb0sw) to import it in your RC. You should then see "sim" model in RC with all channels configured properly. ### Note on Linux Currently default config on Linux is for using Xbox controller. This means other devices might not work properly. In future we will add ability to configure RC in settings.json but for now you *might* have to change code in [SimJoystick.cpp file](/Unreal/Plugins/AirSim/Source/SimJoyStick/SimJoyStick.cpp#L340) to use other devices. @@ -71,4 +72,4 @@ We haven't implemented it yet. This means your RC firmware will need to have a c #### My RC is not working with PX4 setup. -First you want to make sure your RC is working in [QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). If it doesn't then it will sure not work in AirSim. The PX4 mode is suitable for folks who have at least intermediate level of experience to deal with various issues related to PX4 and we would generally refer you to get help from PX4 forums. \ No newline at end of file +First you want to make sure your RC is working in [QGroundControl](https://docs.qgroundcontrol.com/en/SetupView/Radio.html). If it doesn't then it will sure not work in AirSim. The PX4 mode is suitable for folks who have at least intermediate level of experience to deal with various issues related to PX4 and we would generally refer you to get help from PX4 forums. diff --git a/docs/sensors.md b/docs/sensors.md index ebb99e4c2a..5309399158 100644 --- a/docs/sensors.md +++ b/docs/sensors.md @@ -35,7 +35,7 @@ Behind the scenes, `createDefaultSensorSettings` method in [AirSimSettings.hpp]( The default sensor list can be configured in settings json: -```JSON +```json "DefaultSensors": { "Barometer": { "SensorType": 1, @@ -72,98 +72,121 @@ If a vehicle provides its sensor list, it **must** provide the whole list. Selec A vehicle specific sensor list can be specified in the vehicle settings part of the json. e.g., -``` - "Vehicles": { - - "Drone1": { - "VehicleType": "SimpleFlight", - "AutoCreate": true, - ... - "Sensors": { - "MyLidar1": { - "SensorType": 6, - "Enabled" : true, - "NumberOfChannels": 16, - "PointsPerSecond": 10000, - "X": 0, "Y": 0, "Z": -1, - "DrawDebugPoints": true - }, - "MyLidar2": { - "SensorType": 6, - "Enabled" : true, - "NumberOfChannels": 4, - "PointsPerSecond": 10000, - "X": 0, "Y": 0, "Z": -1, - "DrawDebugPoints": true - } +```json +"Vehicles": { + + "Drone1": { + "VehicleType": "SimpleFlight", + "AutoCreate": true, + ... + "Sensors": { + "MyLidar1": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 16, + "PointsPerSecond": 10000, + "X": 0, "Y": 0, "Z": -1, + "DrawDebugPoints": true + }, + "MyLidar2": { + "SensorType": 6, + "Enabled" : true, + "NumberOfChannels": 4, + "PointsPerSecond": 10000, + "X": 0, "Y": 0, "Z": -1, + "DrawDebugPoints": true } } - } + } +} ``` ### Sensor specific settings Each sensor-type has its own set of settings as well. Please see [lidar](lidar.md) for example of Lidar specific settings. +#### Distance Sensor + +By default, Distance Sensor points to the front of the vehicle. It can be pointed in any direction by modifying the settings + +Configurable Parameters - + +Parameter | Description +-----------------|------------ +X Y Z | Position of the sensor relative to the vehicle (in NED, in meters) (Default (0,0,0)-Multirotor, (0,0,-1)-Car) +Yaw Pitch Roll | Orientation of the sensor relative to the vehicle (degrees) (Default (0,0,0)) +MinDistance | Minimum distance measured by distance sensor (metres, only used to fill Mavlink message for PX4) (Default 0.2m) +MaxDistance | Maximum distance measured by distance sensor (metres) (Default 40.0m) + +For example, to make the sensor point towards the ground (for altitude measurement similar to barometer), the orientation can be modified as follows - + +```json +"Distance": { + "SensorType": 5, + "Enabled" : true, + "Yaw": 0, "Pitch": -90, "Roll": 0 +} +``` + +**Note:** For Cars, the sensor is placed 1 meter above the vehicle center by default. This is required since otherwise the sensor gives strange data due it being inside the vehicle. This doesn't affect the sensor values say when measuring the distance between 2 cars. See [`PythonClient/car/distance_sensor_multi.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/car/distance_sensor_multi.py) for an example usage. + +##### Server side visualization for debugging + +Be default, the points hit by distance sensor are not drawn on the viewport. To enable the drawing of hit points on the viewport, please enable setting `DrawDebugPoints` via settings json. E.g. - + +```json +"Distance": { + "SensorType": 5, + "Enabled" : true, + ... + "DrawDebugPoints": true +} +``` + ## Sensor APIs Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/hello_drone.py) or [`hello_drone.cpp`](https://github.com/Microsoft/AirSim/blob/master/HelloDrone/main.cpp) for example usage, or see follow below for the full API. -- Barometer - - C++ - ```cpp - msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name, const std::string& vehicle_name); - ``` - - Python - ```python - barometer_data = getBarometerData(barometer_name = "", vehicle_name = "") - ``` - -- IMU - - C++ - ```cpp - msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = ""); - ``` - - Python - ```python - imu_data = getImuData(imu_name = "", vehicle_name = "") - ``` - -- GPS - - C++ - ```cpp - msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = ""); - ``` - Python - ```python - gps_data = getGpsData(gps_name = "", vehicle_name = "") - ``` - -- Magnetometer - - C++ - ```cpp - msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = ""); - ``` - Python - ```python - magnetometer_data = getMagnetometerData(magnetometer_name = "", vehicle_name = "") - ``` - -- Distance sensor - - C++ - ```cpp - msr::airlib::DistanceBase::Output getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); - ``` - Python - ```python - distance_sensor_name = getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") - ``` - -- Lidar - See [lidar](lidar.md) for Lidar API. +##### Barometer +```cpp +msr::airlib::BarometerBase::Output getBarometerData(const std::string& barometer_name, const std::string& vehicle_name); +``` + +```python +barometer_data = client.getBarometerData(barometer_name = "", vehicle_name = "") +``` + +##### IMU +```cpp +msr::airlib::ImuBase::Output getImuData(const std::string& imu_name = "", const std::string& vehicle_name = ""); +``` + +```python +imu_data = client.getImuData(imu_name = "", vehicle_name = "") +``` + +##### GPS +```cpp +msr::airlib::GpsBase::Output getGpsData(const std::string& gps_name = "", const std::string& vehicle_name = ""); +``` +```python +gps_data = client.getGpsData(gps_name = "", vehicle_name = "") +``` + +##### Magnetometer +```cpp +msr::airlib::MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name = "", const std::string& vehicle_name = ""); +``` +```python +magnetometer_data = client.getMagnetometerData(magnetometer_name = "", vehicle_name = "") +``` + +##### Distance sensor +```cpp +msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); +``` +```python +distance_sensor_data = client.getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") +``` + +##### Lidar +See the [lidar page](lidar.md) for Lidar API. diff --git a/docs/settings.md b/docs/settings.md index 8059e489c6..2d827cae42 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -1,10 +1,18 @@ # AirSim Settings ## Where are Settings Stored? -Windows: `Documents\AirSim` -Linux: `~/Documents/AirSim` +AirSim is searching for the settings definition in 4 different ways in the following order. The first match will be used: -The file is in usual [json format](https://en.wikipedia.org/wiki/JSON). On first startup AirSim would create `settings.json` file with no settings. To avoid problems, always use ASCII format to save json file. +1. Looking at the (absolute) path specified by the `--settings` command line argument. +For example, in Windows: `AirSim.exe --settings 'C:\path\to\settings.json'` +In Linux `./Blocks.sh --settings '/home/$USER/path/to/settings.json'` +1. Looking for a json document passed as a command line argument by the `--settings` argument. +For example, in Windows: `AirSim.exe --settings '{"foo" : "bar"}'` +In Linux `./Blocks.sh --settings '{"foo" : "bar"}'` +1. Looking in the folder of the executable for a file called `settings.json`. +2. Looking in the users home folder for a file called `settings.json`. The AirSim subfolder is located at `Documents\AirSim` on Windows and `~/Documents/AirSim` on Linux systems. + +The file is in usual [json format](https://en.wikipedia.org/wiki/JSON). On first startup AirSim would create `settings.json` file with no settings at the users home folder. To avoid problems, always use ASCII format to save json file. ## How to Chose Between Car and Multirotor? The default is to use multirotor. To use car simple set `"SimMode": "Car"` like this: @@ -212,7 +220,7 @@ The `InitMethod` determines how object IDs are initialized at startup to generat If `OverrideExisting` is false then initialization does not alter non-zero object IDs already assigned otherwise it does. - If `MeshNamingMethod` is "" or "OwnerName" then we use mesh's owner name to generate random hash as object IDs. If its "StaticMeshName" then we use static mesh's name to generate random hash as object IDs. Note that it is not possible to tell individual instances of the same static mesh apart this way, but the names are often more intuitive. + If `MeshNamingMethod` is "" or "OwnerName" then we use mesh's owner name to generate random hash as object IDs. If it is "StaticMeshName" then we use static mesh's name to generate random hash as object IDs. Note that it is not possible to tell individual instances of the same static mesh apart this way, but the names are often more intuitive. ## Camera Settings The `CameraDefaults` element at root level specifies defaults used for all cameras. These defaults can be overridden for individual camera in `Cameras` element inside `Vehicles` as described later. @@ -360,7 +368,7 @@ from the PX4 app. The second channel for controlling the vehicle is defined by To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. Currently this setting applies only to car. ### PawnPaths -This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then choosing "Car Pawn" for Parent Class settings in Class Options. It's also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. +This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then choosing "Car Pawn" for Parent Class settings in Class Options. It is also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. ### PhysicsEngineName For cars, we support only PhysX for now (regardless of value in this setting). For multirotors, we support `"FastPhysicsEngine"` only. @@ -377,4 +385,4 @@ this default is only used when everything you are talking to is contained on a s Unit conversion factor for speed related to `m/s`, default is 1. Used in conjunction with SpeedUnitLabel. This may be only used for display purposes for example on-display speed when car is being driven. For example, to get speed in `miles/hr` use factor 2.23694. ### SpeedUnitLabel -Unit label for speed, default is `m/s`. Used in conjunction with SpeedUnitFactor. \ No newline at end of file +Unit label for speed, default is `m/s`. Used in conjunction with SpeedUnitFactor. diff --git a/docs/unreal_custenv.md b/docs/unreal_custenv.md index d327b0b83d..b528432e22 100644 --- a/docs/unreal_custenv.md +++ b/docs/unreal_custenv.md @@ -23,7 +23,7 @@ There is no `Epic Games Launcher` for Linux which means that if you need to crea 6. Edit the `LandscapeMountains.uproject` so that it looks like this -``` +```json { "FileVersion": 3, "EngineAssociation": "4.24", @@ -108,19 +108,20 @@ In this case, create a new blank C++ project with no Starter Content and add you #### I already have my own Unreal project. How do I use AirSim with it? Copy the `Unreal\Plugins` folder from the build you did in the above section into the root of your Unreal project's folder. In your Unreal project's .uproject file, add the key `AdditionalDependencies` to the "Modules" object as we showed in the `LandscapeMountains.uproject` above. - ``` +```json "AdditionalDependencies": [ "AirSim" ] - ``` +``` + and the `Plugins` section to the top level object: - ``` - "Plugins": [ - { - "Name": "AirSim", - "Enabled": true - } - ] - ``` +```json +"Plugins": [ + { + "Name": "AirSim", + "Enabled": true + } +] +``` diff --git a/mkdocs.yml b/mkdocs.yml index e8385608a1..407e58b7a9 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -9,7 +9,7 @@ markdown_extensions: remote_branch: gh-pages -theme: +theme: name: 'readthedocs' logo: icon: ' ' @@ -22,14 +22,14 @@ extra: palette: social: - type: facebook - link: https://www.facebook.com/groups/1225832467530667/ + link: https://www.facebook.com/groups/1225832467530667/ - type: github-alt link: https://github.com/Microsoft/AirSim copyright: Copyright © 2018 Microsoft Research nav: - - "Home": + - "Home": - "Home": 'README.md' - "Changelog": 'CHANGELOG.md' - "Get AirSim": @@ -37,6 +37,7 @@ nav: - "Build on Windows": 'build_windows.md' - "Build on Linux": 'build_linux.md' - "Docker on Linux": 'docker_ubuntu.md' + - "AirSim on Azure": 'azure.md' - "Custom Unreal Environment": 'unreal_custenv.md' - "AirSim with Unity": "Unity.md" - "Custom Unity Environment": "custom_unity_environments.md" @@ -47,7 +48,7 @@ nav: - "C++ APIs": 'apis_cpp.md' - "Development Workflow": 'dev_workflow.md' - "Settings": 'settings.md' - - "Camera Views": 'camera_views.md' + - "Camera Views": 'camera_views.md' - "Car Mode": 'using_car.md' - "Remote Control": 'remote_control.md' - "XBox Controller": 'xbox_controller.md' @@ -56,8 +57,8 @@ nav: - "Sensors": 'sensors.md' - "LIDAR": 'lidar.md' - "Infrared Camera": "InfraredCamera.md" - - "ROS: AirSim ROS Wrapper": "airsim_ros_pkgs.md" - - "ROS: AirSim Tutorial Packages": "airsim_tutorial_pkgs.md" + - "ROS: AirSim ROS Wrapper": "airsim_ros_pkgs.md" + - "ROS: AirSim Tutorial Packages": "airsim_tutorial_pkgs.md" - "Domain Randomization": "retexturing.md" - "Mesh Vertex Buffers": "meshes.md" - "Playing Logs": 'playback.md' @@ -92,8 +93,9 @@ nav: - "Autonomous Driving on Azure": 'https://aka.ms/AutonomousDrivingCookbook' - "Building Hexacopter": 'https://github.com/Microsoft/AirSim/wiki/hexacopter' - "Moving on Path Demo": 'https://github.com/Microsoft/AirSim/wiki/moveOnPath-demo' - - "Building Point Clouds": 'https://github.com/Microsoft/AirSim/wiki/Point-Clouds' - - "Surveying Using Drone": 'https://github.com/Microsoft/AirSim/wiki/Implementing-a-Drone-Survey-script' + - "Building Point Clouds": 'point_clouds.md' + - "Surveying Using Drone": 'drone_survey.md' + - "Orbit Trajectory": "orbit.md" - "Misc": - "AirSim on Real Drones": 'custom_drone.md' - "Installing cmake on Linux": 'cmake_linux.md' @@ -104,8 +106,8 @@ nav: - "Who is Using AirSim": 'who_is_using.md' - "Working with UE Plugin Contents": 'working_with_plugin_contents.md' - "Formula Student Technion Self-drive": 'https://github.com/Microsoft/AirSim/wiki/technion' - - "Support": + - "Support": - "FAQ": 'faq.md' - "Support": 'SUPPORT.md' - "Create Issue": 'create_issue.md' - - "Contribute": 'CONTRIBUTING.md' \ No newline at end of file + - "Contribute": 'CONTRIBUTING.md' diff --git a/pipelines/ci.yml b/pipelines/ci.yml new file mode 100644 index 0000000000..02d9bfa6bc --- /dev/null +++ b/pipelines/ci.yml @@ -0,0 +1,128 @@ +variables: + container_linux: airsimci.azurecr.io/ue4p25p1/ubuntu18:debugeditor_fulldebugoff + ue4_root_linux: /home/ue4/ue-4.25.1-linux-debugeditor + container_win: airsimci.azurecr.io/ue4p25p1/win1809:pipe + ue4_root_win: C:\ue-4.25.1-win + +stages: + - stage: AirSimCI + jobs: + - job: Ubuntu_1804 + pool: + name: 'AirSim' + demands: + - Spec -equals Ubuntu_18.04 + container: + image: $(container_linux) + endpoint: airsimci_acr_connection + variables: + ue4_root: $(ue4_root_linux) + workspace: + clean: all + steps: + # Setup / Prereq + - checkout: self + - script: | + ./setup.sh + displayName: Install system dependencies + + # Build AirLib + - script: | + ./build.sh + displayName: Build AirLib + + # Build UE Blocks project + - script: | + ./update_from_git.sh + workingDirectory: Unreal/Environments/Blocks + displayName: Copy AirLib to Blocks (update_from_git.sh) + + # Build Blocks + - script: | + $(UE4_ROOT)/Engine/Build/BatchFiles/Linux/Build.sh Blocks Linux Development \ + -project=$(pwd)/Unreal/Environments/Blocks/Blocks.uproject + $(UE4_ROOT)/Engine/Build/BatchFiles/Linux/Build.sh BlocksEditor Linux Development \ + -project=$(pwd)/Unreal/Environments/Blocks/Blocks.uproject + displayName: Build Blocks - Development + + # Package Blocks + - script: | + $(UE4_ROOT)/Engine/Build/BatchFiles/RunUAT.sh BuildCookRun \ + -project="$(pwd)/Unreal/Environments/Blocks/Blocks.uproject" \ + -nop4 -nocompile -build -cook -compressed -pak -allmaps -stage \ + -archive -archivedirectory="$(pwd)/Unreal/Environments/Blocks/Packaged/Development" \ + -clientconfig=Development -clean -utf8output + displayName: Package Blocks - Development + + # Publish Artifact for Blocks Linux + - task: PublishPipelineArtifact@1 + inputs: + targetPath: 'Unreal/Environments/Blocks/Packaged' + artifactName: 'Blocks_Linux' + displayName: Artifact for Blocks Linux + condition: succeededOrFailed() + + - task: ArchiveFiles@2 + displayName: Blocks Linux Zip + inputs: + rootFolderOrFile: 'Unreal/Environments/Blocks/Packaged' + includeRootFolder: false + archiveType: 'zip' + archiveFile: 'Unreal/Environments/Blocks/Packaged/Blocks_Linux.zip' + replaceExistingArchive: true + + - job: Windows_VS2019 + pool: + name: 'AirSim' + demands: + - Spec -equals WinServer2019_VS2019_Datacenter + container: + image: $(container_win) + endpoint: airsimci_acr_connection + variables: + ue4_root: $(ue4_root_win) + workspace: + clean: all + + steps: + - checkout: self + + # Build AirLib + - script: | + call "C:\BuildTools\VC\Auxiliary\Build\vcvars64.bat" + call .\build.cmd + displayName: Build AirLib + + - script: | + call "C:\BuildTools\VC\Auxiliary\Build\vcvars64.bat" + call .\update_from_git_ci.bat + workingDirectory: Unreal/Environments/Blocks + displayName: Copy AirLib to Blocks (update_from_git_ci.bat) + + # Build Blocks + - script: | + call "%UE4_ROOT%\Engine\Build\BatchFiles\Build.bat" Blocks Win64 Development -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" + call "%UE4_ROOT%\Engine\Build\BatchFiles\Build.bat" BlocksEditor Win64 Development -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" + displayName: Build Blocks - Development + + # Package Blocks + - script: | + call "%UE4_ROOT%\Engine\Build\BatchFiles\RunUAT.bat" BuildCookRun -project="%CD%\Unreal\Environments\Blocks\Blocks.uproject" -nop4 -nocompile -build -cook -compressed -pak -allmaps -stage -archive -archivedirectory="%CD%\Unreal\Environments\Blocks\Packaged\Development" -clientconfig=Development -clean -utf8output + displayName: Package Blocks - Development + + # Publish Artifact for Blocks Windows + - task: PublishPipelineArtifact@1 + inputs: + targetPath: 'Unreal/Environments/Blocks/Packaged' + artifactName: 'Blocks_Windows' + displayName: Artifact for Blocks Windows + condition: succeededOrFailed() + + - task: ArchiveFiles@2 + displayName: Blocks Windows Zip + inputs: + rootFolderOrFile: 'Unreal/Environments/Blocks/Packaged' + includeRootFolder: false + archiveType: 'zip' + archiveFile: 'Unreal/Environments/Blocks/Packaged/Blocks_Windows.zip' + replaceExistingArchive: true diff --git a/ros/src/airsim_ros_pkgs/CMakeLists.txt b/ros/src/airsim_ros_pkgs/CMakeLists.txt index 4495e6796b..4131d6c207 100644 --- a/ros/src/airsim_ros_pkgs/CMakeLists.txt +++ b/ros/src/airsim_ros_pkgs/CMakeLists.txt @@ -34,9 +34,12 @@ find_package(catkin REQUIRED COMPONENTS rospy sensor_msgs std_msgs + geographic_msgs + geometry_msgs std_srvs tf2 tf2_ros + tf2_sensor_msgs ) add_message_files( @@ -46,6 +49,10 @@ add_message_files( GPSYaw.msg VelCmd.msg VelCmdGroup.msg + CarControls.msg + CarState.msg + Altimeter.msg + Environment.msg ) add_service_files( @@ -63,6 +70,7 @@ generate_messages( DEPENDENCIES std_msgs geometry_msgs + geographic_msgs ) catkin_package( 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 ad88dc2e9e..958ea1e70f 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -12,6 +12,7 @@ STRICT_MODE_ON #include "ros/ros.h" #include "sensors/imu/ImuBase.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" +#include "vehicles/car/api/CarRpcLibClient.hpp" #include "yaml-cpp/yaml.h" #include #include @@ -23,6 +24,9 @@ STRICT_MODE_ON #include #include #include +#include +#include +#include #include #include #include @@ -43,7 +47,11 @@ STRICT_MODE_ON #include #include #include +#include //hector_uav_msgs defunct? +#include #include +#include +#include #include #include #include @@ -51,7 +59,9 @@ STRICT_MODE_ON #include #include #include +#include #include +#include // #include "nodelet/nodelet.h" // todo move airlib typedefs to separate header file? @@ -115,6 +125,12 @@ struct GimbalCmd class AirsimROSWrapper { public: + enum class AIRSIM_MODE : unsigned + { + DRONE, + CAR + }; + AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string & host_ip); ~AirsimROSWrapper() {}; @@ -127,10 +143,77 @@ class AirsimROSWrapper bool is_used_lidar_timer_cb_queue_; bool is_used_img_timer_cb_queue_; - ros::Time first_imu_ros_ts; - int64_t first_imu_unreal_ts = -1; - private: + struct SensorPublisher + { + SensorBase::SensorType sensor_type; + std::string sensor_name; + ros::Publisher publisher; + }; + + // utility struct for a SINGLE robot + class VehicleROS + { + public: + virtual ~VehicleROS() {} + std::string vehicle_name; + + /// All things ROS + ros::Publisher odom_local_pub; + ros::Publisher global_gps_pub; + ros::Publisher env_pub; + airsim_ros_pkgs::Environment env_msg; + std::vector sensor_pubs; + // handle lidar seperately for max performance as data is collected on its own thread/callback + std::vector lidar_pubs; + + nav_msgs::Odometry curr_odom; + sensor_msgs::NavSatFix gps_sensor_msg; + + std::vector static_tf_msg_vec; + + ros::Time stamp; + + + std::string odom_frame_id; + /// Status + // bool is_armed_; + // std::string mode_; + }; + + class CarROS : public VehicleROS + { + public: + msr::airlib::CarApiBase::CarState curr_car_state; + + ros::Subscriber car_cmd_sub; + ros::Publisher car_state_pub; + airsim_ros_pkgs::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; + // bool in_air_; // todo change to "status" and keep track of this + + ros::Subscriber vel_cmd_body_frame_sub; + ros::Subscriber vel_cmd_world_frame_sub; + + ros::ServiceServer takeoff_srvr; + ros::ServiceServer land_srvr; + + bool has_vel_cmd; + VelCmd vel_cmd; + + /// Status + // bool in_air_; // todo change to "status" and keep track of this + }; + /// ROS timer callbacks void img_response_timer_cb(const ros::TimerEvent& event); // update images from airsim_client_ every nth sec void drone_state_timer_cb(const ros::TimerEvent& event); // update drone state from airsim_client_ every nth sec @@ -150,8 +233,14 @@ class AirsimROSWrapper void gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAngleQuatCmd& gimbal_angle_quat_cmd_msg); void gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg); - ros::Time make_ts(uint64_t unreal_ts); - // void set_zero_vel_cmd(); + // commands + void car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& 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 + ros::Time update_state(); + void update_and_publish_static_transforms(VehicleROS* vehicle_ros); + void publish_vehicle_state(); /// ROS service callbacks bool takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name); @@ -164,7 +253,7 @@ class AirsimROSWrapper /// ROS tf broadcasters void publish_camera_tf(const ImageResponse& img_response, const ros::Time& ros_time, const std::string& frame_id, const std::string& child_frame_id); - void publish_odom_tf(const nav_msgs::Odometry& odom_ned_msg); + void publish_odom_tf(const nav_msgs::Odometry& odom_msg); /// camera helper methods sensor_msgs::CameraInfo generate_cam_info(const std::string& camera_name, const CameraSetting& camera_setting, const CaptureSetting& capture_setting) const; @@ -177,9 +266,9 @@ class AirsimROSWrapper // methods which parse setting json ang generate ros pubsubsrv void create_ros_pubs_from_settings_json(); - void append_static_camera_tf(const std::string& vehicle_name, const std::string& camera_name, const CameraSetting& camera_setting); - void append_static_lidar_tf(const std::string& vehicle_name, const std::string& lidar_name, const LidarSetting& lidar_setting); - void append_static_vehicle_tf(const std::string& vehicle_name, const VehicleSetting& vehicle_setting); + 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 LidarSetting& 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; @@ -188,17 +277,28 @@ class AirsimROSWrapper tf2::Quaternion get_tf2_quat(const msr::airlib::Quaternionr& airlib_quat) const; msr::airlib::Quaternionr get_airlib_quat(const geometry_msgs::Quaternion& geometry_msgs_quat) const; msr::airlib::Quaternionr get_airlib_quat(const tf2::Quaternion& tf2_quat) const; - - nav_msgs::Odometry get_odom_msg_from_airsim_state(const msr::airlib::MultirotorState& drone_state) const; + nav_msgs::Odometry get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const; + nav_msgs::Odometry get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const; + airsim_ros_pkgs::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_ros_pkgs::GPSYaw get_gps_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; sensor_msgs::NavSatFix get_gps_sensor_msg_from_airsim_geo_point(const msr::airlib::GeoPoint& geo_point) const; - sensor_msgs::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data); - sensor_msgs::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data) const; + sensor_msgs::Imu get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const; + airsim_ros_pkgs::Altimeter get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const; + sensor_msgs::Range get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const; + sensor_msgs::PointCloud2 get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const; + sensor_msgs::NavSatFix get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const; + sensor_msgs::MagneticField get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const; + airsim_ros_pkgs::Environment get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const; // 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::CameraInfo& cam_info) const; void convert_yaml_to_simple_mat(const YAML::Node& node, SimpleMatrix& m) const; // todo ugly + // simulation time utility + 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; + private: // subscriber / services for ALL robots ros::Subscriber vel_cmd_all_body_frame_sub_; @@ -212,80 +312,52 @@ class AirsimROSWrapper ros::ServiceServer takeoff_group_srvr_; ros::ServiceServer land_group_srvr_; - // utility struct for a SINGLE robot - struct MultiRotorROS - { - std::string vehicle_name; - - /// All things ROS - ros::Publisher odom_local_ned_pub; - ros::Publisher global_gps_pub; - // ros::Publisher home_geo_point_pub_; // geo coord of unreal origin - - ros::Subscriber vel_cmd_body_frame_sub; - ros::Subscriber vel_cmd_world_frame_sub; - - ros::ServiceServer takeoff_srvr; - ros::ServiceServer land_srvr; - - /// State - msr::airlib::MultirotorState curr_drone_state; - // bool in_air_; // todo change to "status" and keep track of this - nav_msgs::Odometry curr_odom_ned; - sensor_msgs::NavSatFix gps_sensor_msg; - bool has_vel_cmd; - VelCmd vel_cmd; - - std::string odom_frame_id; - /// Status - // bool in_air_; // todo change to "status" and keep track of this - // bool is_armed_; - // std::string mode_; - }; + AIRSIM_MODE airsim_mode_ = AIRSIM_MODE::DRONE; ros::ServiceServer reset_srvr_; ros::Publisher origin_geo_point_pub_; // home geo coord of drones msr::airlib::GeoPoint origin_geo_point_;// gps coord of unreal origin airsim_ros_pkgs::GPSYaw origin_geo_point_msg_; // todo duplicate - std::vector multirotor_ros_vec_; - - std::vector vehicle_names_; std::vector vehicle_setting_vec_; AirSimSettingsParser airsim_settings_parser_; - std::unordered_map vehicle_name_idx_map_; + std::unordered_map< std::string, std::unique_ptr< VehicleROS > > vehicle_name_ptr_map_; static const std::unordered_map image_type_int_to_string_map_; - std::map vehicle_imu_map_; - std::map vehicle_lidar_map_; - std::vector static_tf_msg_vec_; + bool is_vulkan_; // rosparam obtained from launch file. If vulkan is being used, we BGR encoding instead of RGB - msr::airlib::MultirotorRpcLibClient airsim_client_; - msr::airlib::MultirotorRpcLibClient airsim_client_images_; - msr::airlib::MultirotorRpcLibClient airsim_client_lidar_; + 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 MultiRotorROS? + // todo for multiple drones with multiple sensors, this won't scale. make it a part of VehicleROS? ros::CallbackQueue img_timer_cb_queue_; ros::CallbackQueue lidar_timer_cb_queue_; - // todo race condition - std::recursive_mutex drone_control_mutex_; - // std::recursive_mutex img_mutex_; - // std::recursive_mutex lidar_mutex_; + std::mutex drone_control_mutex_; // gimbal control bool has_gimbal_cmd_; GimbalCmd gimbal_cmd_; /// ROS tf - std::string world_frame_id_; + 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; tf2_ros::TransformBroadcaster tf_broadcaster_; tf2_ros::StaticTransformBroadcaster static_tf_pub_; + + bool isENU_ = false; tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; /// ROS params double vel_cmd_duration_; @@ -299,13 +371,13 @@ class AirsimROSWrapper std::vector airsim_img_request_vehicle_name_pair_vec_; std::vector image_pub_vec_; std::vector cam_info_pub_vec_; - std::vector lidar_pub_vec_; - std::vector imu_pub_vec_; std::vector camera_info_msg_vec_; /// ROS other publishers ros::Publisher clock_pub_; + rosgraph_msgs::Clock ros_clock_; + bool publish_clock_ = false; ros::Subscriber gimbal_angle_quat_cmd_sub_; ros::Subscriber gimbal_angle_euler_cmd_sub_; diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch new file mode 100644 index 0000000000..682558b656 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch new file mode 100644 index 0000000000..621e04c343 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/launch/airsim_car_with_joy_control_auto.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/launch/airsim_node.launch b/ros/src/airsim_ros_pkgs/launch/airsim_node.launch index 2512a5b9f7..53acec90cd 100644 --- a/ros/src/airsim_ros_pkgs/launch/airsim_node.launch +++ b/ros/src/airsim_ros_pkgs/launch/airsim_node.launch @@ -1,14 +1,19 @@ + + + - - + + + - + + - \ No newline at end of file + diff --git a/ros/src/airsim_ros_pkgs/msg/Altimeter.msg b/ros/src/airsim_ros_pkgs/msg/Altimeter.msg new file mode 100644 index 0000000000..3a3cd1eeda --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/Altimeter.msg @@ -0,0 +1,4 @@ +Header header +float32 altitude +float32 pressure +float32 qnh \ No newline at end of file diff --git a/ros/src/airsim_ros_pkgs/msg/CarControls.msg b/ros/src/airsim_ros_pkgs/msg/CarControls.msg new file mode 100644 index 0000000000..3dffecb229 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/CarControls.msg @@ -0,0 +1,8 @@ +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/ros/src/airsim_ros_pkgs/msg/CarState.msg b/ros/src/airsim_ros_pkgs/msg/CarState.msg new file mode 100644 index 0000000000..4c780ad94d --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/CarState.msg @@ -0,0 +1,8 @@ +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/ros/src/airsim_ros_pkgs/msg/Environment.msg b/ros/src/airsim_ros_pkgs/msg/Environment.msg new file mode 100644 index 0000000000..e0c1ea7247 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/msg/Environment.msg @@ -0,0 +1,8 @@ +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/ros/src/airsim_ros_pkgs/package.xml b/ros/src/airsim_ros_pkgs/package.xml index 6b8c98c187..af9228a1ae 100644 --- a/ros/src/airsim_ros_pkgs/package.xml +++ b/ros/src/airsim_ros_pkgs/package.xml @@ -15,11 +15,16 @@ image_transport message_generation message_runtime + mavros_msgs nav_msgs roscpp rospy sensor_msgs std_msgs + geographic_msgs + geometry_msgs + tf2_sensor_msgs + geometry_msgs @@ -31,9 +36,10 @@ rospy sensor_msgs std_msgs + joy - \ No newline at end of file + diff --git a/ros/src/airsim_ros_pkgs/rviz/default.rviz b/ros/src/airsim_ros_pkgs/rviz/default.rviz index 5aebb67634..7e19fe3b0c 100644 --- a/ros/src/airsim_ros_pkgs/rviz/default.rviz +++ b/ros/src/airsim_ros_pkgs/rviz/default.rviz @@ -7,8 +7,9 @@ Panels: - /TF1 - /TF1/Frames1 - /Odometry1/Shape1 - Splitter Ratio: 0.50515461 - Tree Height: 775 + - /PointCloud21 + Splitter Ratio: 0.5051546096801758 + Tree Height: 847 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -17,7 +18,7 @@ Panels: - /2D Nav Goal1 - /Publish Point1 Name: Tool Properties - Splitter Ratio: 0.588679016 + Splitter Ratio: 0.5886790156364441 - Class: rviz/Views Expanded: - /Current View1 @@ -27,7 +28,11 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: front_left_depth_planar_registered_point_cloud + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -37,7 +42,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.0299999993 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -54,16 +59,16 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: false - LidarCustom: + drone_1: Value: true - MyQuad: + drone_1/LidarCustom: Value: true - MyQuad/odom_local_ned: + drone_1/odom_local_enu: Value: true front_center_custom_body: Value: false front_center_custom_body/static: - Value: false + Value: true front_center_custom_optical: Value: true front_center_custom_optical/static: @@ -71,7 +76,7 @@ Visualization Manager: front_left_custom_body: Value: false front_left_custom_body/static: - Value: false + Value: true front_left_custom_optical: Value: true front_left_custom_optical/static: @@ -79,7 +84,7 @@ Visualization Manager: front_right_custom_body: Value: false front_right_custom_body/static: - Value: false + Value: true front_right_custom_optical: Value: true front_right_custom_optical/static: @@ -95,36 +100,35 @@ Visualization Manager: Show Names: true Tree: world_ned: - MyQuad: - MyQuad/odom_local_ned: - LidarCustom: - {} - front_center_custom_body/static: + world_enu: + drone_1: + drone_1/odom_local_enu: + drone_1/LidarCustom: + {} + front_center_custom_body/static: + {} + front_center_custom_optical/static: + {} + front_left_custom_body/static: + {} + front_left_custom_optical/static: + {} + front_right_custom_body/static: + {} + front_right_custom_optical/static: + {} + front_center_custom_body: {} - front_center_custom_optical/static: + front_center_custom_optical: {} - front_left_custom_body/static: + front_left_custom_body: {} - front_left_custom_optical/static: + front_left_custom_optical: {} - front_right_custom_body/static: + front_right_custom_body: {} - front_right_custom_optical/static: + front_right_custom_optical: {} - front_center_custom_body: - {} - front_center_custom_optical: - {} - front_left_custom_body: - {} - front_left_custom_optical: - {} - front_right_custom_body: - {} - front_right_custom_optical: - {} - world_enu: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -150,14 +154,14 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.100000001 + Size (m): 0.10000000149011612 Style: Flat Squares Topic: /airsim_node/front_left_custom/DepthPlanner/registered/points Unreliable: false Use Fixed Frame: true Use rainbow: true Value: true - - Angle Tolerance: 0.100000001 + - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: @@ -169,7 +173,7 @@ Visualization Manager: Scale: 1 Value: true Position: - Alpha: 0.300000012 + Alpha: 0.30000001192092896 Color: 204; 51; 204 Scale: 1 Value: true @@ -177,16 +181,16 @@ Visualization Manager: Enabled: true Keep: 25 Name: Odometry - Position Tolerance: 0.100000001 + Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 - Axes Radius: 0.100000001 + Axes Radius: 0.10000000149011612 Color: 255; 25; 0 - Head Length: 0.200000003 - Head Radius: 0.0500000007 - Shaft Length: 0.200000003 - Shaft Radius: 0.0199999996 + Head Length: 0.20000000298023224 + Head Radius: 0.05000000074505806 + Shaft Length: 0.20000000298023224 + Shaft Radius: 0.019999999552965164 Value: Arrow Topic: /airsim_node/MyQuad/odom_local_ned Unreliable: false @@ -214,9 +218,9 @@ Visualization Manager: Queue Size: 10 Selectable: true Size (Pixels): 3 - Size (m): 0.00999999978 + Size (m): 0.009999999776482582 Style: Flat Squares - Topic: /airsim_node/MyQuad/lidar/LidarCustom + Topic: /airsim_node/drone_1/lidar/LidarCustom Unreliable: false Use Fixed Frame: true Use rainbow: true @@ -236,7 +240,10 @@ Visualization Manager: - Class: rviz/FocusCamera - Class: rviz/Measure - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 - Class: rviz/SetGoal Topic: /move_base_simple/goal - Class: rviz/PublishPoint @@ -246,33 +253,33 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 98.1204224 + Distance: 34.531036376953125 Enable Stereo Rendering: - Stereo Eye Separation: 0.0599999987 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.0908625796 - Y: -0.00513405167 - Z: -0.783257008 + X: 0.08613172918558121 + Y: 0.2811238765716553 + Z: -0.7929707169532776 Focal Shape Fixed Size: false - Focal Shape Size: 0.0500000007 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.00999999978 - Pitch: 0.63979739 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.34479764103889465 Target Frame: world_view Value: Orbit (rviz) - Yaw: 3.76538205 + Yaw: 3.170380115509033 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1138 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Time: @@ -281,6 +288,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1875 - X: 1965 - Y: 24 + Width: 1918 + X: 1050 + Y: 278 diff --git a/ros/src/airsim_ros_pkgs/scripts/car_joy b/ros/src/airsim_ros_pkgs/scripts/car_joy new file mode 100755 index 0000000000..b8fab4ef48 --- /dev/null +++ b/ros/src/airsim_ros_pkgs/scripts/car_joy @@ -0,0 +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() + node.run() \ No newline at end of file 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 ddee05c290..5b775c0c5f 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -3,6 +3,8 @@ // #include // PLUGINLIB_EXPORT_CLASS(AirsimROSWrapper, nodelet::Nodelet) #include "common/AirSimSettings.hpp" +#include + constexpr char AirsimROSWrapper::CAM_YML_NAME[]; constexpr char AirsimROSWrapper::WIDTH_YML_NAME[]; @@ -24,26 +26,34 @@ const std::unordered_map AirsimROSWrapper::image_type_int_to_s { 7, "Infrared" } }; -AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string & host_ip) : +AirsimROSWrapper::AirsimROSWrapper(const ros::NodeHandle& nh, const ros::NodeHandle& nh_private, const std::string& 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_), // same as above, but for lidar - airsim_client_(host_ip), + host_ip_(host_ip), airsim_client_images_(host_ip), - airsim_client_lidar_(host_ip) + airsim_client_lidar_(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; - world_frame_id_ = "world_ned"; // todo rosparam? - + if (AirSimSettings::singleton().simmode_name != "Car") + { + airsim_mode_ = AIRSIM_MODE::DRONE; + ROS_INFO("Setting ROS wrapper to DRONE mode"); + } + else + { + airsim_mode_ = AIRSIM_MODE::CAR; + ROS_INFO("Setting ROS wrapper to CAR mode"); + } + initialize_ros(); std::cout << "AirsimROSWrapper Initialized!\n"; - // intitialize placeholder control commands - // vel_cmd_ = VelCmd(); - // gimbal_cmd_ = GimbalCmd(); } void AirsimROSWrapper::initialize_airsim() @@ -51,17 +61,26 @@ void AirsimROSWrapper::initialize_airsim() // todo do not reset if already in air? try { - airsim_client_.confirmConnection(); + + if (airsim_mode_ == AIRSIM_MODE::DRONE) + { + airsim_client_ = std::unique_ptr(new msr::airlib::MultirotorRpcLibClient(host_ip_)); + } + else + { + airsim_client_ = std::move(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 : vehicle_names_) + for (const auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - airsim_client_.enableApiControl(true, vehicle_name); // todo expose as rosservice? - airsim_client_.armDisarm(true, vehicle_name); // todo exposes as rosservice? + 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_ = airsim_client_.getHomeGeoPoint(""); + origin_geo_point_ = airsim_client_->getHomeGeoPoint(""); // 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_); } @@ -79,6 +98,12 @@ void AirsimROSWrapper::initialize_ros() double update_airsim_control_every_n_sec; nh_private_.getParam("is_vulkan", is_vulkan_); nh_private_.getParam("update_airsim_control_every_n_sec", update_airsim_control_every_n_sec); + nh_private_.getParam("publish_clock", publish_clock_); + nh_private_.param("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_private_.param("odom_frame_id", odom_frame_id_, odom_frame_id_); + isENU_ = !(odom_frame_id_ == AIRSIM_ODOM_FRAME_ID); + nh_private_.param("coordinate_system_enu", isENU_, isENU_); vel_cmd_duration_ = 0.05; // todo rosparam // todo enforce dynamics constraints in this node as well? // nh_.getParam("max_vert_vel_", max_vert_vel_); @@ -100,60 +125,73 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() image_pub_vec_.clear(); cam_info_pub_vec_.clear(); camera_info_msg_vec_.clear(); - static_tf_msg_vec_.clear(); - imu_pub_vec_.clear(); - lidar_pub_vec_.clear(); - vehicle_names_.clear(); // todo should eventually support different types of vehicles in a single instance - // vehicle_setting_vec_.clear(); - // vehicle_imu_map_; - multirotor_ros_vec_.clear(); - // callback_queues_.clear(); + vehicle_name_ptr_map_.clear(); + size_t lidar_cnt = 0; image_transport::ImageTransport image_transporter(nh_private_); - int idx = 0; // 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; - vehicle_names_.push_back(curr_vehicle_name); set_nans_to_zeros_in_pose(*vehicle_setting); - // auto vehicle_setting_local = vehicle_setting.get(); - - append_static_vehicle_tf(curr_vehicle_name, *vehicle_setting); - vehicle_name_idx_map_[curr_vehicle_name] = idx; // allows fast lookup in command callbacks in case of a lot of drones - - MultiRotorROS multirotor_ros; - multirotor_ros.odom_frame_id = curr_vehicle_name + "/odom_local_ned"; - multirotor_ros.vehicle_name = curr_vehicle_name; - multirotor_ros.odom_local_ned_pub = nh_private_.advertise(curr_vehicle_name + "/odom_local_ned", 10); - multirotor_ros.global_gps_pub = nh_private_.advertise(curr_vehicle_name + "/global_gps", 10); - - // 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 - multirotor_ros.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, multirotor_ros.vehicle_name)); // todo ros::TransportHints().tcpNoDelay(); - multirotor_ros.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, multirotor_ros.vehicle_name)); - - multirotor_ros.takeoff_srvr = nh_private_.advertiseService(curr_vehicle_name + "/takeoff", - boost::bind(&AirsimROSWrapper::takeoff_srv_cb, this, _1, _2, multirotor_ros.vehicle_name) ); - multirotor_ros.land_srvr = nh_private_.advertiseService(curr_vehicle_name + "/land", - boost::bind(&AirsimROSWrapper::land_srv_cb, this, _1, _2, multirotor_ros.vehicle_name) ); - // multirotor_ros.reset_srvr = nh_private_.advertiseService(curr_vehicle_name + "/reset",&AirsimROSWrapper::reset_srv_cb, this); - - multirotor_ros_vec_.push_back(multirotor_ros); - idx++; - - // iterate over camera map std::map cameras; + + 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); + + vehicle_ros->odom_local_pub = nh_private_.advertise(curr_vehicle_name + "/" + odom_frame_id_, 10); + + vehicle_ros->env_pub = nh_private_.advertise(curr_vehicle_name + "/environment", 10); + + vehicle_ros->global_gps_pub = nh_private_.advertise(curr_vehicle_name + "/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 + 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_state_pub = nh_private_.advertise(curr_vehicle_name + "/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; // vehicle_setting_vec_.push_back(*vehicle_setting.get()); set_nans_to_zeros_in_pose(*vehicle_setting, camera_setting); - append_static_camera_tf(curr_vehicle_name, curr_camera_name, 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(); @@ -189,62 +227,84 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() } - // iterate over sensors std::map> sensors; + // iterate over sensors + std::vector sensors; for (auto& curr_sensor_map : vehicle_setting->sensors) { auto& sensor_name = curr_sensor_map.first; auto& sensor_setting = curr_sensor_map.second; - switch (sensor_setting->sensor_type) - { - case SensorBase::SensorType::Barometer: - { - std::cout << "Barometer" << std::endl; - break; - } - case SensorBase::SensorType::Imu: + if (sensor_setting->enabled) + { + SensorPublisher sensor_publisher; + sensor_publisher.sensor_name = sensor_setting->sensor_name; + sensor_publisher.sensor_type = sensor_setting->sensor_type; + switch (sensor_setting->sensor_type) { - vehicle_imu_map_[curr_vehicle_name] = sensor_name; - // todo this is pretty non scalable, refactor airsim and ros api and maintain a vehicle <-> sensor (setting) map - std::cout << "Imu" << std::endl; - imu_pub_vec_.push_back(nh_private_.advertise (curr_vehicle_name + "/imu/" + sensor_name, 10)); - break; - } - case SensorBase::SensorType::Gps: - { - std::cout << "Gps" << std::endl; - break; - } - case SensorBase::SensorType::Magnetometer: - { - std::cout << "Magnetometer" << std::endl; - break; - } - case SensorBase::SensorType::Distance: - { - std::cout << "Distance" << std::endl; - break; - } - case SensorBase::SensorType::Lidar: - { - std::cout << "Lidar" << std::endl; - auto lidar_setting = *static_cast(sensor_setting.get()); - set_nans_to_zeros_in_pose(*vehicle_setting, lidar_setting); - append_static_lidar_tf(curr_vehicle_name, sensor_name, lidar_setting); // todo is there a more readable way to down-cast? - vehicle_lidar_map_[curr_vehicle_name] = sensor_name; // non scalable - lidar_pub_vec_.push_back(nh_private_.advertise (curr_vehicle_name + "/lidar/" + sensor_name, 10)); - break; - } - default: - { - throw std::invalid_argument("Unexpected sensor type"); + case SensorBase::SensorType::Barometer: + { + std::cout << "Barometer" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/altimeter/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Imu: + { + std::cout << "Imu" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/imu/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Gps: + { + std::cout << "Gps" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/gps/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Magnetometer: + { + std::cout << "Magnetometer" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/magnetometer/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Distance: + { + std::cout << "Distance" << std::endl; + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/distance/" + sensor_name, 10); + break; + } + case SensorBase::SensorType::Lidar: + { + std::cout << "Lidar" << std::endl; + auto lidar_setting = *static_cast(sensor_setting.get()); + set_nans_to_zeros_in_pose(*vehicle_setting, lidar_setting); + append_static_lidar_tf(vehicle_ros.get(), sensor_name, lidar_setting); + sensor_publisher.publisher = nh_private_.advertise(curr_vehicle_name + "/lidar/" + sensor_name, 10); + break; + } + default: + { + throw std::invalid_argument("Unexpected sensor type"); + } } + sensors.emplace_back(sensor_publisher); } } + + // we want fast access to the lidar sensors for callback handling, sort them out now + 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); + vehicle_ros->sensor_pubs.resize(sensors.size() - cnt); + std::partition_copy (sensors.begin(), sensors.end(), vehicle_ros->lidar_pubs.begin(), vehicle_ros->sensor_pubs.begin(), isLidar); + + 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 (multirotor_ros_vec_.size() > 1) + if (vehicle_name_ptr_map_.size() > 1 && airsim_mode_ == AIRSIM_MODE::DRONE) { takeoff_all_srvr_ = nh_private_.advertiseService("all_robots/takeoff", &AirsimROSWrapper::takeoff_all_srv_cb, this); land_all_srvr_ = nh_private_.advertiseService("all_robots/land", &AirsimROSWrapper::land_all_srv_cb, this); @@ -264,135 +324,120 @@ void AirsimROSWrapper::create_ros_pubs_from_settings_json() // todo add per vehicle reset in AirLib API reset_srvr_ = nh_private_.advertiseService("reset",&AirsimROSWrapper::reset_srv_cb, this); - // todo mimic gazebo's /use_sim_time feature which publishes airsim's clock time..via an rpc call?! - // clock_pub_ = nh_private_.advertise("clock", 10); + if (publish_clock_) + { + clock_pub_ = nh_private_.advertise("clock", 1); + } // if >0 cameras, add one more thread for img_request_timer_cb - if(airsim_img_request_vehicle_name_pair_vec_.size() > 0) + if(!airsim_img_request_vehicle_name_pair_vec_.empty()) { 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); - bool separate_spinner = true; // todo debugging race condition - if(separate_spinner) - { - 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; - } - else - { - airsim_img_response_timer_ = nh_private_.createTimer(ros::Duration(update_airsim_img_response_every_n_sec), &AirsimROSWrapper::img_response_timer_cb, this); - } + + 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; } - if (lidar_pub_vec_.size() > 0) + // lidars update on their own callback/thread at a given rate + if (lidar_cnt > 0) { 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_); - bool separate_spinner = true; // todo debugging race condition - if(separate_spinner) - { - 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; - } - else - { - airsim_lidar_update_timer_ = nh_private_.createTimer(ros::Duration(update_lidar_every_n_sec), &AirsimROSWrapper::lidar_timer_cb, this); - } + 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; } initialize_airsim(); } -ros::Time AirsimROSWrapper::make_ts(uint64_t unreal_ts) { - if (first_imu_unreal_ts < 0) { - first_imu_unreal_ts = unreal_ts; - first_imu_ros_ts = ros::Time::now(); - } - return first_imu_ros_ts + ros::Duration( (unreal_ts- first_imu_unreal_ts)/1e9); -} - - // todo: error check. if state is not landed, return error. bool AirsimROSWrapper::takeoff_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - airsim_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 - airsim_client_.takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = + return true; } bool AirsimROSWrapper::takeoff_group_srv_cb(airsim_ros_pkgs::TakeoffGroup::Request& request, airsim_ros_pkgs::TakeoffGroup::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) for(const auto& vehicle_name : request.vehicle_names) - airsim_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) - airsim_client_.takeoffAsync(20, vehicle_name); + static_cast(airsim_client_.get())->takeoffAsync(20, vehicle_name); // response.success = + return true; } bool AirsimROSWrapper::takeoff_all_srv_cb(airsim_ros_pkgs::Takeoff::Request& request, airsim_ros_pkgs::Takeoff::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.takeoffAsync(20, vehicle_name)->waitOnLastTask(); // todo value for timeout_sec? + 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 : vehicle_names_) - airsim_client_.takeoffAsync(20, vehicle_name); + 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(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - airsim_client_.landAsync(60, vehicle_name)->waitOnLastTask(); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name)->waitOnLastTask(); else - airsim_client_.landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + return true; //todo } bool AirsimROSWrapper::land_group_srv_cb(airsim_ros_pkgs::LandGroup::Request& request, airsim_ros_pkgs::LandGroup::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) for(const auto& vehicle_name : request.vehicle_names) - airsim_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) - airsim_client_.landAsync(60, vehicle_name); + static_cast(airsim_client_.get())->landAsync(60, vehicle_name); + return true; //todo } bool AirsimROSWrapper::land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, airsim_ros_pkgs::Land::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); if (request.waitOnLastTask) - for(const auto& vehicle_name : vehicle_names_) - airsim_client_.landAsync(60, vehicle_name)->waitOnLastTask(); + 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 : vehicle_names_) - airsim_client_.landAsync(60, vehicle_name); + 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 } @@ -400,7 +445,7 @@ bool AirsimROSWrapper::land_all_srv_cb(airsim_ros_pkgs::Land::Request& request, // todo not async remove waitonlasttask bool AirsimROSWrapper::reset_srv_cb(airsim_ros_pkgs::Reset::Request& request, airsim_ros_pkgs::Reset::Response& response) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); airsim_client_.reset(); return true; //todo @@ -421,123 +466,146 @@ msr::airlib::Quaternionr AirsimROSWrapper::get_airlib_quat(const tf2::Quaternion return msr::airlib::Quaternionr(tf2_quat.w(), tf2_quat.x(), tf2_quat.y(), tf2_quat.z()); } +void AirsimROSWrapper::car_cmd_cb(const airsim_ros_pkgs::CarControls::ConstPtr& msg, const std::string& vehicle_name) +{ + std::lock_guard guard(drone_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_ros_pkgs::VelCmd& msg, const std::string& vehicle_name) void AirsimROSWrapper::vel_cmd_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); - - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + std::lock_guard guard(drone_control_mutex_); + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg->twist.linear.x * cos(yaw)) - (msg->twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg->twist.linear.x * sin(yaw)) + (msg->twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg->twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg->twist.linear.x * cos(yaw)) - (msg->twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg->twist.linear.x * sin(yaw)) + (msg->twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg->twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); + drone->has_vel_cmd = true; } void AirsimROSWrapper::vel_cmd_group_body_frame_cb(const airsim_ros_pkgs::VelCmdGroup& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); for(const auto& vehicle_name : msg.vehicle_names) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + double roll, pitch, yaw; + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } // void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg) void AirsimROSWrapper::vel_cmd_all_body_frame_cb(const airsim_ros_pkgs::VelCmd& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); // todo expose waitOnLastTask or nah? - for(const auto& vehicle_name : vehicle_names_) + for(auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + double roll, pitch, yaw; - tf2::Matrix3x3(get_tf2_quat(multirotor_ros_vec_[vehicle_idx].curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw + tf2::Matrix3x3(get_tf2_quat(drone->curr_drone_state.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw // todo do actual body frame? - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.x = (msg.twist.linear.x * cos(yaw)) - (msg.twist.linear.y * sin(yaw)); //body frame assuming zero pitch roll + drone->vel_cmd.y = (msg.twist.linear.x * sin(yaw)) + (msg.twist.linear.y * cos(yaw)); //body frame + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; // airsim uses degrees - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } void AirsimROSWrapper::vel_cmd_world_frame_cb(const airsim_ros_pkgs::VelCmd::ConstPtr& msg, const std::string& vehicle_name) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg->twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg->twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg->twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + drone->vel_cmd.x = msg->twist.linear.x; + drone->vel_cmd.y = msg->twist.linear.y; + drone->vel_cmd.z = msg->twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg->twist.angular.z); + 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_ros_pkgs::VelCmdGroup& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); for(const auto& vehicle_name : msg.vehicle_names) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg.twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg.twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + auto drone = static_cast(vehicle_name_ptr_map_[vehicle_name].get()); + + drone->vel_cmd.x = msg.twist.linear.x; + drone->vel_cmd.y = msg.twist.linear.y; + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } void AirsimROSWrapper::vel_cmd_all_world_frame_cb(const airsim_ros_pkgs::VelCmd& msg) { - std::lock_guard guard(drone_control_mutex_); + std::lock_guard guard(drone_control_mutex_); // todo expose waitOnLastTask or nah? - for(const auto& vehicle_name : vehicle_names_) + for(auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - int vehicle_idx = vehicle_name_idx_map_[vehicle_name]; - - multirotor_ros_vec_[vehicle_idx].vel_cmd.x = msg.twist.linear.x; - multirotor_ros_vec_[vehicle_idx].vel_cmd.y = msg.twist.linear.y; - multirotor_ros_vec_[vehicle_idx].vel_cmd.z = msg.twist.linear.z; - multirotor_ros_vec_[vehicle_idx].vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.is_rate = true; - multirotor_ros_vec_[vehicle_idx].vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); - multirotor_ros_vec_[vehicle_idx].has_vel_cmd = true; + auto drone = static_cast(vehicle_name_ptr_pair.second.get()); + + drone->vel_cmd.x = msg.twist.linear.x; + drone->vel_cmd.y = msg.twist.linear.y; + drone->vel_cmd.z = msg.twist.linear.z; + drone->vel_cmd.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; + drone->vel_cmd.yaw_mode.is_rate = true; + drone->vel_cmd.yaw_mode.yaw_or_rate = math_common::rad2deg(msg.twist.angular.z); + drone->has_vel_cmd = true; } } @@ -563,7 +631,7 @@ void AirsimROSWrapper::gimbal_angle_quat_cmd_cb(const airsim_ros_pkgs::GimbalAng // 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 setcameraorientation which sets camera orientation wrt world (or takeoff?) ned frame. todo +// 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_ros_pkgs::GimbalAngleEulerCmd& gimbal_angle_euler_cmd_msg) { try @@ -582,37 +650,99 @@ void AirsimROSWrapper::gimbal_angle_euler_cmd_cb(const airsim_ros_pkgs::GimbalAn } } -nav_msgs::Odometry AirsimROSWrapper::get_odom_msg_from_airsim_state(const msr::airlib::MultirotorState& drone_state) const +airsim_ros_pkgs::CarState AirsimROSWrapper::get_roscarstate_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const { - nav_msgs::Odometry odom_ned_msg; - // odom_ned_msg.header.frame_id = world_frame_id_; - // odom_ned_msg.child_frame_id = "/airsim/odom_local_ned"; // todo make param - - odom_ned_msg.pose.pose.position.x = drone_state.getPosition().x(); - odom_ned_msg.pose.pose.position.y = drone_state.getPosition().y(); - odom_ned_msg.pose.pose.position.z = drone_state.getPosition().z(); - odom_ned_msg.pose.pose.orientation.x = drone_state.getOrientation().x(); - odom_ned_msg.pose.pose.orientation.y = drone_state.getOrientation().y(); - odom_ned_msg.pose.pose.orientation.z = drone_state.getOrientation().z(); - odom_ned_msg.pose.pose.orientation.w = drone_state.getOrientation().w(); - - odom_ned_msg.twist.twist.linear.x = drone_state.kinematics_estimated.twist.linear.x(); - odom_ned_msg.twist.twist.linear.y = drone_state.kinematics_estimated.twist.linear.y(); - odom_ned_msg.twist.twist.linear.z = drone_state.kinematics_estimated.twist.linear.z(); - odom_ned_msg.twist.twist.angular.x = drone_state.kinematics_estimated.twist.angular.x(); - odom_ned_msg.twist.twist.angular.y = drone_state.kinematics_estimated.twist.angular.y(); - odom_ned_msg.twist.twist.angular.z = drone_state.kinematics_estimated.twist.angular.z(); - - return odom_ned_msg; + airsim_ros_pkgs::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::Odometry AirsimROSWrapper::get_odom_msg_from_car_state(const msr::airlib::CarApiBase::CarState& car_state) const +{ + nav_msgs::Odometry odom_msg; + + odom_msg.pose.pose.position.x = car_state.getPosition().x(); + odom_msg.pose.pose.position.y = car_state.getPosition().y(); + odom_msg.pose.pose.position.z = car_state.getPosition().z(); + odom_msg.pose.pose.orientation.x = car_state.getOrientation().x(); + odom_msg.pose.pose.orientation.y = car_state.getOrientation().y(); + odom_msg.pose.pose.orientation.z = car_state.getOrientation().z(); + odom_msg.pose.pose.orientation.w = car_state.getOrientation().w(); + + odom_msg.twist.twist.linear.x = car_state.kinematics_estimated.twist.linear.x(); + odom_msg.twist.twist.linear.y = car_state.kinematics_estimated.twist.linear.y(); + odom_msg.twist.twist.linear.z = car_state.kinematics_estimated.twist.linear.z(); + odom_msg.twist.twist.angular.x = car_state.kinematics_estimated.twist.angular.x(); + odom_msg.twist.twist.angular.y = car_state.kinematics_estimated.twist.angular.y(); + odom_msg.twist.twist.angular.z = car_state.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::Odometry AirsimROSWrapper::get_odom_msg_from_multirotor_state(const msr::airlib::MultirotorState& drone_state) const +{ + nav_msgs::Odometry odom_msg; + + odom_msg.pose.pose.position.x = drone_state.getPosition().x(); + odom_msg.pose.pose.position.y = drone_state.getPosition().y(); + odom_msg.pose.pose.position.z = drone_state.getPosition().z(); + odom_msg.pose.pose.orientation.x = drone_state.getOrientation().x(); + odom_msg.pose.pose.orientation.y = drone_state.getOrientation().y(); + odom_msg.pose.pose.orientation.z = drone_state.getOrientation().z(); + odom_msg.pose.pose.orientation.w = drone_state.getOrientation().w(); + + odom_msg.twist.twist.linear.x = drone_state.kinematics_estimated.twist.linear.x(); + odom_msg.twist.twist.linear.y = drone_state.kinematics_estimated.twist.linear.y(); + odom_msg.twist.twist.linear.z = drone_state.kinematics_estimated.twist.linear.z(); + odom_msg.twist.twist.angular.x = drone_state.kinematics_estimated.twist.angular.x(); + odom_msg.twist.twist.angular.y = drone_state.kinematics_estimated.twist.angular.y(); + odom_msg.twist.twist.angular.z = drone_state.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; } // 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::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data) const +sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr::airlib::LidarData& lidar_data, const std::string& vehicle_name) const { sensor_msgs::PointCloud2 lidar_msg; - lidar_msg.header.frame_id = world_frame_id_; // todo + lidar_msg.header.stamp = ros::Time::now(); + lidar_msg.header.frame_id = vehicle_name; if (lidar_data.point_cloud.size() > 3) { @@ -623,6 +753,7 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: 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) @@ -639,7 +770,7 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: lidar_msg.is_dense = true; // todo std::vector data_std = lidar_data.point_cloud; - const unsigned char* bytes = reinterpret_cast(&data_std[0]); + 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); } @@ -647,14 +778,107 @@ sensor_msgs::PointCloud2 AirsimROSWrapper::get_lidar_msg_from_airsim(const msr:: { // msg = [] } + + if (isENU_) + { + try + { + sensor_msgs::PointCloud2 lidar_msg_enu; + auto transformStampedENU = tf_buffer_.lookupTransform(AIRSIM_FRAME_ID, vehicle_name, ros::Time(0), ros::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) + { + ROS_WARN("%s", ex.what()); + ros::Duration(1.0).sleep(); + } + } + return lidar_msg; } +airsim_ros_pkgs::Environment AirsimROSWrapper::get_environment_msg_from_airsim(const msr::airlib::Environment::State& env_data) const +{ + airsim_ros_pkgs::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::MagneticField AirsimROSWrapper::get_mag_msg_from_airsim(const msr::airlib::MagnetometerBase::Output& mag_data) const +{ + sensor_msgs::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::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) +sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airlib::GpsBase::Output& gps_data) const +{ + sensor_msgs::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::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::Range AirsimROSWrapper::get_range_from_airsim(const msr::airlib::DistanceSensorData& dist_data) const +{ + sensor_msgs::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_ros_pkgs::Altimeter AirsimROSWrapper::get_altimeter_msg_from_airsim(const msr::airlib::BarometerBase::Output& alt_data) const +{ + airsim_ros_pkgs::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::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::ImuBase::Output& imu_data) const { sensor_msgs::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(); @@ -670,7 +894,6 @@ sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::Im imu_msg.linear_acceleration.y = imu_data.linear_acceleration.y(); imu_msg.linear_acceleration.z = imu_data.linear_acceleration.z(); - imu_msg.header.stamp = make_ts(imu_data.time_stamp); // imu_msg.orientation_covariance = ; // imu_msg.angular_velocity_covariance = ; // imu_msg.linear_acceleration_covariance = ; @@ -678,18 +901,18 @@ sensor_msgs::Imu AirsimROSWrapper::get_imu_msg_from_airsim(const msr::airlib::Im return imu_msg; } -void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_ned_msg) +void AirsimROSWrapper::publish_odom_tf(const nav_msgs::Odometry& odom_msg) { geometry_msgs::TransformStamped odom_tf; - odom_tf.header = odom_ned_msg.header; - odom_tf.child_frame_id = odom_ned_msg.child_frame_id; - odom_tf.transform.translation.x = odom_ned_msg.pose.pose.position.x; - odom_tf.transform.translation.y = odom_ned_msg.pose.pose.position.y; - odom_tf.transform.translation.z = odom_ned_msg.pose.pose.position.z; - odom_tf.transform.rotation.x = odom_ned_msg.pose.pose.orientation.x; - odom_tf.transform.rotation.y = odom_ned_msg.pose.pose.orientation.y; - odom_tf.transform.rotation.z = odom_ned_msg.pose.pose.orientation.z; - odom_tf.transform.rotation.w = odom_ned_msg.pose.pose.orientation.w; + 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.x = odom_msg.pose.pose.orientation.x; + odom_tf.transform.rotation.y = odom_msg.pose.pose.orientation.y; + odom_tf.transform.rotation.z = odom_msg.pose.pose.orientation.z; + odom_tf.transform.rotation.w = odom_msg.pose.pose.orientation.w; tf_broadcaster_.sendTransform(odom_tf); } @@ -711,112 +934,266 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_sensor_msg_from_airsim_geo_poin return gps_msg; } -// todo unused -// void AirsimROSWrapper::set_zero_vel_cmd() -// { -// vel_cmd_.x = 0.0; -// vel_cmd_.y = 0.0; -// vel_cmd_.z = 0.0; - -// vel_cmd_.drivetrain = msr::airlib::DrivetrainType::MaxDegreeOfFreedom; -// vel_cmd_.yaw_mode.is_rate = false; +ros::Time AirsimROSWrapper::chrono_timestamp_to_ros(const std::chrono::system_clock::time_point& stamp) const +{ + auto dur = std::chrono::duration(stamp.time_since_epoch()); + ros::Time cur_time; + cur_time.fromSec(dur.count()); + return cur_time; +} -// // todo make class member or a fucntion -// double roll, pitch, yaw; -// tf2::Matrix3x3(get_tf2_quat(curr_drone_state_.kinematics_estimated.pose.orientation)).getRPY(roll, pitch, yaw); // ros uses xyzw -// vel_cmd_.yaw_mode.yaw_or_rate = yaw; -// } +ros::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); + ros::Time cur_time = chrono_timestamp_to_ros(tp); + return cur_time; +} void AirsimROSWrapper::drone_state_timer_cb(const ros::TimerEvent& event) { try - { - std::lock_guard guard(drone_control_mutex_); - + { // todo this is global origin origin_geo_point_pub_.publish(origin_geo_point_msg_); - // iterate over drones - for (auto& multirotor_ros: multirotor_ros_vec_) + + // 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()) { - // get drone state from airsim - std::unique_lock lck(drone_control_mutex_); - multirotor_ros.curr_drone_state = airsim_client_.getMultirotorState(multirotor_ros.vehicle_name); - lck.unlock(); - ros::Time curr_ros_time = ros::Time::now(); - - // convert airsim drone state to ROS msgs - multirotor_ros.curr_odom_ned = get_odom_msg_from_airsim_state(multirotor_ros.curr_drone_state); - multirotor_ros.curr_odom_ned.header.frame_id = multirotor_ros.vehicle_name; - multirotor_ros.curr_odom_ned.child_frame_id = multirotor_ros.odom_frame_id; - multirotor_ros.curr_odom_ned.header.stamp = curr_ros_time; - - multirotor_ros.gps_sensor_msg = get_gps_sensor_msg_from_airsim_geo_point(multirotor_ros.curr_drone_state.gps_location); - multirotor_ros.gps_sensor_msg.header.stamp = curr_ros_time; - - // publish to ROS! - multirotor_ros.odom_local_ned_pub.publish(multirotor_ros.curr_odom_ned); - publish_odom_tf(multirotor_ros.curr_odom_ned); - multirotor_ros.global_gps_pub.publish(multirotor_ros.gps_sensor_msg); + // 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_); + } - // send control commands from the last callback to airsim - if (multirotor_ros.has_vel_cmd) - { - std::unique_lock lck(drone_control_mutex_); - airsim_client_.moveByVelocityAsync(multirotor_ros.vel_cmd.x, multirotor_ros.vel_cmd.y, multirotor_ros.vel_cmd.z, vel_cmd_duration_, - msr::airlib::DrivetrainType::MaxDegreeOfFreedom, multirotor_ros.vel_cmd.yaw_mode, multirotor_ros.vehicle_name); - lck.unlock(); - } + // 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::cout << "error" << std::endl; + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API:" << std::endl << msg << std::endl; + } +} - // "clear" control cmds - multirotor_ros.has_vel_cmd = false; +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); } + } +} - // IMUS - if (imu_pub_vec_.size() > 0) +ros::Time AirsimROSWrapper::update_state() +{ + bool got_sim_time = false; + ros::Time curr_ros_time = ros::Time::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_) + { + ros::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) { - int ctr = 0; - for (const auto& vehicle_imu_pair: vehicle_imu_map_) + 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) { - std::unique_lock lck(drone_control_mutex_); - auto imu_data = airsim_client_.getImuData(vehicle_imu_pair.second, vehicle_imu_pair.first); - lck.unlock(); - sensor_msgs::Imu imu_msg = get_imu_msg_from_airsim(imu_data); - imu_msg.header.frame_id = vehicle_imu_pair.first; - // imu_msg.header.stamp = ros::Time::now(); - imu_pub_vec_[ctr].publish(imu_msg); - ctr++; - } - } + 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; - if (static_tf_msg_vec_.size() > 0) + vehicle_ros->curr_odom = get_odom_msg_from_multirotor_state(drone->curr_drone_state); + } + else { - for (auto& static_tf_msg : static_tf_msg_vec_) + 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) { - static_tf_msg.header.stamp = ros::Time::now(); - static_tf_pub_.sendTransform(static_tf_msg); + curr_ros_time = vehicle_time; + got_sim_time = true; } - // we've sent these static transforms, so no need to keep sending them - static_tf_msg_vec_.clear(); + 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_ros_pkgs::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_ros_pkgs::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); } - // todo add and expose a gimbal angular velocity to airlib - if (has_gimbal_cmd_) + // 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->sensor_pubs) { - std::unique_lock lck(drone_control_mutex_); - airsim_client_.simSetCameraOrientation(gimbal_cmd_.camera_name, gimbal_cmd_.target_quat, gimbal_cmd_.vehicle_name); - lck.unlock(); + switch (sensor_publisher.sensor_type) + { + case SensorBase::SensorType::Barometer: + { + auto baro_data = airsim_client_->getBarometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + airsim_ros_pkgs::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); + break; + } + case SensorBase::SensorType::Imu: + { + auto imu_data = airsim_client_->getImuData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::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); + break; + } + case SensorBase::SensorType::Distance: + { + auto distance_data = airsim_client_->getDistanceSensorData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::Range dist_msg = get_range_from_airsim(distance_data); + dist_msg.header.frame_id = vehicle_ros->vehicle_name; + sensor_publisher.publisher.publish(dist_msg); + break; + } + case SensorBase::SensorType::Gps: + { + auto gps_data = airsim_client_->getGpsData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::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); + break; + } + case SensorBase::SensorType::Lidar: + { + // handled via callback + break; + } + case SensorBase::SensorType::Magnetometer: + { + auto mag_data = airsim_client_->getMagnetometerData(sensor_publisher.sensor_name, vehicle_ros->vehicle_name); + sensor_msgs::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); + break; + } + } } - has_gimbal_cmd_ = false; + update_and_publish_static_transforms(vehicle_ros.get()); } +} - catch (rpc::rpc_error& e) +void AirsimROSWrapper::update_commands() +{ + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - std::cout << "error" << std::endl; - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API:" << std::endl << msg << std::endl; + 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(drone_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(drone_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(drone_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 @@ -885,15 +1262,15 @@ void AirsimROSWrapper::set_nans_to_zeros_in_pose(const VehicleSetting& vehicle_s lidar_setting.rotation.roll = vehicle_setting.rotation.roll; } -void AirsimROSWrapper::append_static_vehicle_tf(const std::string& vehicle_name, const VehicleSetting& vehicle_setting) +void AirsimROSWrapper::append_static_vehicle_tf(VehicleROS* vehicle_ros, const VehicleSetting& vehicle_setting) { geometry_msgs::TransformStamped vehicle_tf_msg; vehicle_tf_msg.header.frame_id = world_frame_id_; vehicle_tf_msg.header.stamp = ros::Time::now(); - vehicle_tf_msg.child_frame_id = vehicle_name; + vehicle_tf_msg.child_frame_id = vehicle_ros->vehicle_name; vehicle_tf_msg.transform.translation.x = vehicle_setting.position.x(); vehicle_tf_msg.transform.translation.y = vehicle_setting.position.y(); - vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z(); + vehicle_tf_msg.transform.translation.z = vehicle_setting.position.z(); tf2::Quaternion quat; quat.setRPY(vehicle_setting.rotation.roll, vehicle_setting.rotation.pitch, vehicle_setting.rotation.yaw); vehicle_tf_msg.transform.rotation.x = quat.x(); @@ -901,15 +1278,22 @@ void AirsimROSWrapper::append_static_vehicle_tf(const std::string& vehicle_name, vehicle_tf_msg.transform.rotation.z = quat.z(); vehicle_tf_msg.transform.rotation.w = quat.w(); - static_tf_msg_vec_.push_back(vehicle_tf_msg); + if (isENU_) + { + std::swap(vehicle_tf_msg.transform.translation.x, vehicle_tf_msg.transform.translation.y); + std::swap(vehicle_tf_msg.transform.rotation.x, vehicle_tf_msg.transform.rotation.y); + vehicle_tf_msg.transform.translation.z = -vehicle_tf_msg.transform.translation.z; + vehicle_tf_msg.transform.rotation.z = -vehicle_tf_msg.transform.rotation.z; + } + + vehicle_ros->static_tf_msg_vec.emplace_back(vehicle_tf_msg); } -void AirsimROSWrapper::append_static_lidar_tf(const std::string& vehicle_name, const std::string& lidar_name, const LidarSetting& lidar_setting) +void AirsimROSWrapper::append_static_lidar_tf(VehicleROS* vehicle_ros, const std::string& lidar_name, const LidarSetting& lidar_setting) { - geometry_msgs::TransformStamped lidar_tf_msg; - lidar_tf_msg.header.frame_id = vehicle_name + "/odom_local_ned"; // todo multiple drones - lidar_tf_msg.child_frame_id = lidar_name; + 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.translation.x = lidar_setting.position.x(); lidar_tf_msg.transform.translation.y = lidar_setting.position.y(); lidar_tf_msg.transform.translation.z = lidar_setting.position.z(); @@ -920,13 +1304,21 @@ void AirsimROSWrapper::append_static_lidar_tf(const std::string& vehicle_name, c lidar_tf_msg.transform.rotation.z = quat.z(); lidar_tf_msg.transform.rotation.w = quat.w(); - static_tf_msg_vec_.push_back(lidar_tf_msg); + if (isENU_) + { + std::swap(lidar_tf_msg.transform.translation.x, lidar_tf_msg.transform.translation.y); + std::swap(lidar_tf_msg.transform.rotation.x, lidar_tf_msg.transform.rotation.y); + lidar_tf_msg.transform.translation.z = -lidar_tf_msg.transform.translation.z; + lidar_tf_msg.transform.rotation.z = -lidar_tf_msg.transform.rotation.z; + } + + vehicle_ros->static_tf_msg_vec.emplace_back(lidar_tf_msg); } -void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, const std::string& camera_name, const CameraSetting& camera_setting) +void AirsimROSWrapper::append_static_camera_tf(VehicleROS* vehicle_ros, const std::string& camera_name, const CameraSetting& camera_setting) { geometry_msgs::TransformStamped static_cam_tf_body_msg; - static_cam_tf_body_msg.header.frame_id = vehicle_name + "/odom_local_ned"; + 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.translation.x = camera_setting.position.x(); static_cam_tf_body_msg.transform.translation.y = camera_setting.position.y(); @@ -938,6 +1330,14 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, static_cam_tf_body_msg.transform.rotation.z = quat.z(); static_cam_tf_body_msg.transform.rotation.w = quat.w(); + if (isENU_) + { + std::swap(static_cam_tf_body_msg.transform.translation.x, static_cam_tf_body_msg.transform.translation.y); + std::swap(static_cam_tf_body_msg.transform.rotation.x, static_cam_tf_body_msg.transform.rotation.y); + static_cam_tf_body_msg.transform.translation.z = -static_cam_tf_body_msg.transform.translation.z; + static_cam_tf_body_msg.transform.rotation.z = -static_cam_tf_body_msg.transform.rotation.z; + } + geometry_msgs::TransformStamped static_cam_tf_optical_msg = static_cam_tf_body_msg; static_cam_tf_optical_msg.child_frame_id = camera_name + "_optical/static"; @@ -953,8 +1353,8 @@ void AirsimROSWrapper::append_static_camera_tf(const std::string& vehicle_name, quat_cam_optical.normalize(); tf2::convert(quat_cam_optical, static_cam_tf_optical_msg.transform.rotation); - static_tf_msg_vec_.push_back(static_cam_tf_body_msg); - static_tf_msg_vec_.push_back(static_cam_tf_optical_msg); + 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(const ros::TimerEvent& event) @@ -964,9 +1364,7 @@ void AirsimROSWrapper::img_response_timer_cb(const ros::TimerEvent& event) int image_response_idx = 0; for (const auto& airsim_img_request_vehicle_name_pair : airsim_img_request_vehicle_name_pair_vec_) { - std::unique_lock lck(drone_control_mutex_); const std::vector& img_response = airsim_client_images_.simGetImages(airsim_img_request_vehicle_name_pair.first, airsim_img_request_vehicle_name_pair.second); - lck.unlock(); if (img_response.size() == airsim_img_request_vehicle_name_pair.first.size()) { @@ -988,26 +1386,19 @@ void AirsimROSWrapper::lidar_timer_cb(const ros::TimerEvent& event) { try { - // std::lock_guard guard(drone_control_mutex_); - if (lidar_pub_vec_.size() > 0) + for (auto& vehicle_name_ptr_pair : vehicle_name_ptr_map_) { - // std::lock_guard guard(lidar_mutex_); - int ctr = 0; - for (const auto& vehicle_lidar_pair: vehicle_lidar_map_) + if (!vehicle_name_ptr_pair.second->lidar_pubs.empty()) { - std::unique_lock lck(drone_control_mutex_); - auto lidar_data = airsim_client_lidar_.getLidarData(vehicle_lidar_pair.second, vehicle_lidar_pair.first); // airsim api is imu_name, vehicle_name - lck.unlock(); - sensor_msgs::PointCloud2 lidar_msg = get_lidar_msg_from_airsim(lidar_data); // todo make const ptr msg to avoid copy - lidar_msg.header.frame_id = vehicle_lidar_pair.second; // sensor frame name. todo add to doc - lidar_msg.header.stamp = ros::Time::now(); - lidar_pub_vec_[ctr].publish(lidar_msg); - ctr++; - } + 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::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(); @@ -1034,7 +1425,7 @@ sensor_msgs::ImagePtr AirsimROSWrapper::get_img_msg_from_response(const ImageRes sensor_msgs::ImagePtr img_msg_ptr = boost::make_shared(); img_msg_ptr->data = img_response.image_data_uint8; img_msg_ptr->step = img_response.width * 3; // todo un-hardcode. image_width*num_bytes - img_msg_ptr->header.stamp = make_ts(img_response.time_stamp); + 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; @@ -1053,7 +1444,7 @@ sensor_msgs::ImagePtr AirsimROSWrapper::get_depth_img_msg_from_response(const Im // 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); sensor_msgs::ImagePtr depth_img_msg = cv_bridge::CvImage(std_msgs::Header(), "32FC1", depth_img).toImageMsg(); - depth_img_msg->header.stamp = make_ts(img_response.time_stamp); + 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; } @@ -1087,10 +1478,6 @@ void AirsimROSWrapper::process_and_publish_img_response(const std::vector/dev/null @@ -27,6 +28,15 @@ if [ "$(uname)" == "Darwin" ]; then # osx brew tap llvm-hs/homebrew-llvm brew install llvm@8 else #linux + sudo apt-get update + sudo apt-get -y install --no-install-recommends \ + lsb-release \ + rsync \ + software-properties-common \ + wget \ + libvulkan1 \ + vulkan-utils + #install clang and build tools VERSION=$(lsb_release -rs | cut -d. -f1) # Since Ubuntu 17 clang is part of the core repository @@ -38,16 +48,28 @@ else #linux sudo apt-get install -y clang-8 clang++-8 libc++-8-dev libc++abi-8-dev fi +if ! which cmake; then + # CMake not installed + cmake_ver=0 +else + cmake_ver=$(cmake --version 2>&1 | head -n1 | cut -d ' ' -f3 | awk '{print $NF}') +fi + #give user perms to access USB port - this is not needed if not using PX4 HIL #TODO: figure out how to do below in travis +# Install additional tools, CMake if required if [ "$(uname)" == "Darwin" ]; then # osx if [[ ! -z "${whoami}" ]]; then #this happens when running in travis sudo dseditgroup -o edit -a `whoami` -t user dialout fi - brew install wget - brew install coreutils - brew install cmake # should get cmake 3.8 + brew install wget coreutils + + if version_less_than_equal_to $cmake_ver $MIN_CMAKE_VERSION; then + brew install cmake # should get cmake 3.8 + else + echo "Already have good version of cmake: $cmake_ver" + fi else #linux if [[ ! -z "${whoami}" ]]; then #this happens when running in travis @@ -55,36 +77,46 @@ else #linux sudo usermod -a -G dialout $USER fi - #install additional tools - sudo apt-get install -y build-essential - sudo apt-get install -y unzip -fi + # install additional tools + sudo apt-get install -y build-essential unzip + + if version_less_than_equal_to $cmake_ver $MIN_CMAKE_VERSION; then + # in ubuntu 18 docker CI, avoid building cmake from scratch to save time + # ref: https://apt.kitware.com/ + if [ "$(lsb_release -rs)" == "18.04" ]; then + sudo apt-get -y install \ + apt-transport-https \ + ca-certificates \ + gnupg + wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null + sudo apt-add-repository 'deb https://apt.kitware.com/ubuntu/ bionic main' + sudo apt-get -y install --no-install-recommends \ + make \ + cmake + + else + # For Ubuntu 16.04, or anything else, build CMake 3.10.2 from source + if [[ ! -d "cmake_build/bin" ]]; then + echo "Downloading cmake..." + wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ + -O cmake.tar.gz + tar -xzf cmake.tar.gz + rm cmake.tar.gz + rm -rf ./cmake_build + mv ./cmake-3.10.2 ./cmake_build + pushd cmake_build + ./bootstrap + make + popd + fi + fi + + else + echo "Already have good version of cmake: $cmake_ver" + fi -if ! which cmake; then - # CMake not installed - cmake_ver=0 -else - cmake_ver=$(cmake --version 2>&1 | head -n1 | cut -d ' ' -f3 | awk '{print $NF}') -fi +fi # End USB setup, CMake install -#download cmake - v3.10.2 is not out of box in Ubuntu 16.04 -if version_less_than_equal_to $cmake_ver $MIN_CMAKE_VERSION; then - if [[ ! -d "cmake_build/bin" ]]; then - echo "Downloading cmake..." - wget https://cmake.org/files/v3.10/cmake-3.10.2.tar.gz \ - -O cmake.tar.gz - tar -xzf cmake.tar.gz - rm cmake.tar.gz - rm -rf ./cmake_build - mv ./cmake-3.10.2 ./cmake_build - pushd cmake_build - ./bootstrap - make - popd - fi -else - echo "Already have good version of cmake: $cmake_ver" -fi # Download rpclib if [ ! -d "external/rpclib/rpclib-2.2.1" ]; then @@ -92,13 +124,13 @@ if [ ! -d "external/rpclib/rpclib-2.2.1" ]; then echo "Downloading rpclib..." echo "*********************************************************************************************" - wget https://github.com/rpclib/rpclib/archive/v2.2.1.zip + wget https://github.com/madratman/rpclib/archive/v2.2.1.zip # remove previous versions rm -rf "external/rpclib" mkdir -p "external/rpclib" - unzip v2.2.1.zip -d external/rpclib + unzip -q v2.2.1.zip -d external/rpclib rm v2.2.1.zip fi @@ -122,7 +154,7 @@ if $downloadHighPolySuv; then if [ -d "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" ]; then rm -rf "../Unreal/Plugins/AirSim/Content/VehicleAdv/SUV" fi - unzip car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv + unzip -q car_assets.zip -d ../Unreal/Plugins/AirSim/Content/VehicleAdv cd .. rm -rf "suv_download_tmp" fi @@ -135,7 +167,7 @@ echo "Installing Eigen library..." if [ ! -d "AirLib/deps/eigen3" ]; then echo "Downloading Eigen..." wget -O eigen3.zip https://gitlab.com/libeigen/eigen/-/archive/3.3.7/eigen-3.3.7.zip - unzip eigen3.zip -d temp_eigen + unzip -q eigen3.zip -d temp_eigen mkdir -p AirLib/deps/eigen3 mv temp_eigen/eigen*/Eigen AirLib/deps/eigen3 rm -rf temp_eigen diff --git a/tools/install_ros_deps.sh b/tools/install_ros_deps.sh index e6bddda375..4480dfbe9a 100755 --- a/tools/install_ros_deps.sh +++ b/tools/install_ros_deps.sh @@ -30,4 +30,5 @@ fi sudo apt-get install gcc-8 g++-8 sudo apt-get install ros-$ROS_DISTRO-mavros* +sudo apt-get install ros-$ROS_DISTRO-tf2-sensor-msgs sudo apt-get install python-catkin-tools