From fd895ea0acca0520b7e3c06341e3cfa0fdac7abf Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Fri, 24 Jul 2020 12:26:35 +0530 Subject: [PATCH] Distance sensor Upgrades (#2807) * Add DrawDebugPoint option for distance sensor * Expose Distance Sensor settings Plus fix for Car sensor default pose * [Pythonclient] Fix DistanceSensorData type Distances are float, not Quaternion * [docs] Add info about Distance sensor settings and usage * Use custom sensor orientation in Mavlink Distance sensor message * [ros] Change to DistanceSensorData struct * [Pythonclient] Example script for distance sensor with 2 cars --- AirLib/include/api/RpcLibAdapatorsBase.hpp | 6 +- AirLib/include/api/RpcLibClientBase.hpp | 2 +- AirLib/include/api/VehicleApiBase.hpp | 2 +- AirLib/include/common/AirSimSettings.hpp | 14 +++- AirLib/include/common/CommonStructs.hpp | 11 ++++ .../include/sensors/distance/DistanceBase.hpp | 16 +---- .../sensors/distance/DistanceSimple.hpp | 15 +++-- .../sensors/distance/DistanceSimpleParams.hpp | 36 ++++++++++- .../mavlink/MavLinkMultirotorApi.hpp | 15 +++-- AirLib/src/api/RpcLibClientBase.cpp | 2 +- PythonClient/airsim/types.py | 6 +- PythonClient/car/distance_sensor_multi.py | 49 ++++++++++++++ .../AirSim/Source/SimMode/SimModeBase.cpp | 64 +++++++++++++++++-- .../AirSim/Source/SimMode/SimModeBase.h | 7 +- docs/sensors.md | 54 +++++++++++++--- .../include/airsim_ros_wrapper.h | 3 +- .../src/airsim_ros_wrapper.cpp | 2 +- 17 files changed, 246 insertions(+), 58 deletions(-) create mode 100644 PythonClient/car/distance_sensor_multi.py diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp index 2295d75710..13304cc8f8 100644 --- a/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -672,7 +672,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; @@ -681,9 +681,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 5e3a0845ae..7f98072743 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -84,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; // sensor omniscient APIs vector simGetLidarSegmentation(const std::string& lidar_name = "", const std::string& vehicle_name = "") const; diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index ece0099b43..fa90fcaf21 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -215,7 +215,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 { const DistanceBase* distance_sensor = nullptr; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 4e91492e51..348f235c8d 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -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 { @@ -1161,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 82ba47b3f7..405848e3b8 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -304,6 +304,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/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/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/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 89b7a52982..be9c01fe80 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(); } diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 25e359843c..14f73f1682 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -400,9 +400,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/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index e86f100223..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" @@ -304,6 +305,8 @@ void ASimModeBase::Tick(float DeltaSeconds) drawLidarDebugPoints(); + drawDistanceSensorDebugPoints(); + Super::Tick(DeltaSeconds); } @@ -538,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()); @@ -670,12 +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; @@ -685,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; @@ -694,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 @@ -715,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 b357a6c1fb..1b0b678ffe 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h @@ -128,6 +128,10 @@ class AIRSIM_API ASimModeBase : public AActor 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 @@ -170,4 +174,5 @@ class AIRSIM_API ASimModeBase : public AActor void setupPhysicsLoopPeriod(); void showClockStats(); void drawLidarDebugPoints(); -}; \ No newline at end of file + void drawDistanceSensorDebugPoints(); +}; diff --git a/docs/sensors.md b/docs/sensors.md index ebb99e4c2a..5f81bf037f 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,7 +72,7 @@ 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., -``` +```json "Vehicles": { "Drone1": { @@ -105,6 +105,44 @@ e.g., 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. @@ -117,7 +155,7 @@ Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/mas Python ```python - barometer_data = getBarometerData(barometer_name = "", vehicle_name = "") + barometer_data = client.getBarometerData(barometer_name = "", vehicle_name = "") ``` - IMU @@ -129,7 +167,7 @@ Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/mas Python ```python - imu_data = getImuData(imu_name = "", vehicle_name = "") + imu_data = client.getImuData(imu_name = "", vehicle_name = "") ``` - GPS @@ -140,7 +178,7 @@ Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/mas ``` Python ```python - gps_data = getGpsData(gps_name = "", vehicle_name = "") + gps_data = client.getGpsData(gps_name = "", vehicle_name = "") ``` - Magnetometer @@ -151,18 +189,18 @@ Jump straight to [`hello_drone.py`](https://github.com/Microsoft/AirSim/blob/mas ``` Python ```python - magnetometer_data = getMagnetometerData(magnetometer_name = "", vehicle_name = "") + magnetometer_data = client.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 = ""); + msr::airlib::DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name = "", const std::string& vehicle_name = ""); ``` Python ```python - distance_sensor_name = getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") + distance_sensor_data = client.getDistanceSensorData(distance_sensor_name = "", vehicle_name = "") ``` - Lidar 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 5ff3023c04..958ea1e70f 100644 --- a/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h +++ b/ros/src/airsim_ros_pkgs/include/airsim_ros_wrapper.h @@ -11,7 +11,6 @@ STRICT_MODE_ON #include "common/common_utils/FileSystem.hpp" #include "ros/ros.h" #include "sensors/imu/ImuBase.hpp" -#include "sensors/distance/DistanceBase.hpp" #include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp" #include "vehicles/car/api/CarRpcLibClient.hpp" #include "yaml-cpp/yaml.h" @@ -286,7 +285,7 @@ class AirsimROSWrapper 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) 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::DistanceBase::Output& dist_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; 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 db388bceab..5b775c0c5f 100644 --- a/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp +++ b/ros/src/airsim_ros_pkgs/src/airsim_ros_wrapper.cpp @@ -851,7 +851,7 @@ sensor_msgs::NavSatFix AirsimROSWrapper::get_gps_msg_from_airsim(const msr::airl return gps_msg; } -sensor_msgs::Range AirsimROSWrapper::get_range_from_airsim(const msr::airlib::DistanceBase::Output& dist_data) const +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);