From fc10eb1eea198935e56bc711b1a54e44269e94fb Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 29 Jun 2020 15:07:11 +0530 Subject: [PATCH 1/4] Combine Lidar Segmentation API into getLidarData --- AirLib/include/api/RpcLibAdapatorsBase.hpp | 5 +++- AirLib/include/api/RpcLibClientBase.hpp | 3 --- AirLib/include/api/VehicleApiBase.hpp | 9 ------- AirLib/include/common/CommonStructs.hpp | 7 +++++ AirLib/include/sensors/lidar/LidarBase.hpp | 27 -------------------- AirLib/include/sensors/lidar/LidarSimple.hpp | 4 +-- AirLib/src/api/RpcLibClientBase.cpp | 5 ---- AirLib/src/api/RpcLibServerBase.cpp | 5 ---- 8 files changed, 13 insertions(+), 52 deletions(-) diff --git a/AirLib/include/api/RpcLibAdapatorsBase.hpp b/AirLib/include/api/RpcLibAdapatorsBase.hpp index 69717d1f24..7d8aa683c4 100644 --- a/AirLib/include/api/RpcLibAdapatorsBase.hpp +++ b/AirLib/include/api/RpcLibAdapatorsBase.hpp @@ -465,8 +465,9 @@ class RpcLibAdapatorsBase { msr::airlib::TTimePoint time_stamp; // timestamp std::vector point_cloud; // data Pose pose; + std::vector segmentation; - MSGPACK_DEFINE_MAP(time_stamp, point_cloud, pose); + MSGPACK_DEFINE_MAP(time_stamp, point_cloud, pose, segmentation); LidarData() {} @@ -476,6 +477,7 @@ class RpcLibAdapatorsBase { time_stamp = s.time_stamp; point_cloud = s.point_cloud; pose = s.pose; + segmentation = s.segmentation; } msr::airlib::LidarData to() const @@ -485,6 +487,7 @@ class RpcLibAdapatorsBase { d.time_stamp = time_stamp; d.point_cloud = point_cloud; d.pose = pose.to(); + d.segmentation = segmentation; return d; } diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index 6c1a30db01..1d17fe9a66 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/AirLib/include/api/RpcLibClientBase.hpp @@ -89,9 +89,6 @@ class RpcLibClientBase { msr::airlib::GpsBase::Output getGpsData(const std::string& gps_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; - Pose simGetVehiclePose(const std::string& vehicle_name = "") const; void simSetVehiclePose(const Pose& pose, bool ignore_collision, const std::string& vehicle_name = ""); void simSetTraceLine(const std::vector& color_rgba, float thickness=3.0f, const std::string& vehicle_name = ""); diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index fa90fcaf21..b9d1608ff8 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -119,15 +119,6 @@ class VehicleApiBase : public UpdatableObject { return lidar->getOutput(); } - virtual vector getLidarSegmentation(const std::string& lidar_name) const - { - auto *lidar = findLidarByName(lidar_name); - if (lidar == nullptr) - throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str())); - - return lidar->getSegmentationOutput(); - } - // IMU API virtual ImuBase::Output getImuData(const std::string& imu_name) const { diff --git a/AirLib/include/common/CommonStructs.hpp b/AirLib/include/common/CommonStructs.hpp index 405848e3b8..835a76a84a 100644 --- a/AirLib/include/common/CommonStructs.hpp +++ b/AirLib/include/common/CommonStructs.hpp @@ -297,8 +297,15 @@ struct RCData { struct LidarData { TTimePoint time_stamp = 0; + // data + // - array of floats that represent [x,y,z] coordinate for each point hit within the range + // x0, y0, z0, x1, y1, z1, ..., xn, yn, zn + // TODO: Do we need an intensity place-holder [x,y,z, intensity]? + // - in lidar local NED coordinates + // - in meters vector point_cloud; Pose pose; + vector segmentation; LidarData() {} diff --git a/AirLib/include/sensors/lidar/LidarBase.hpp b/AirLib/include/sensors/lidar/LidarBase.hpp index 05a286f513..21770fb7e9 100644 --- a/AirLib/include/sensors/lidar/LidarBase.hpp +++ b/AirLib/include/sensors/lidar/LidarBase.hpp @@ -14,22 +14,6 @@ class LidarBase : public SensorBase { : SensorBase(sensor_name) {} -public: //types - struct Output { //fields to enable creation of ROS message PointCloud2 and LaserScan - - // header - TTimePoint time_stamp; - Pose relative_pose; - - // data - // - array of floats that represent [x,y,z] coordinate for each point hit within the range - // x0, y0, z0, x1, y1, z1, ..., xn, yn, zn - // TODO: Do we need an intensity place-holder [x,y,z, intensity]? - // - in lidar local NED coordinates - // - in meters - vector point_cloud; - }; - public: virtual void reportState(StateReporter& reporter) override { @@ -45,25 +29,14 @@ class LidarBase : public SensorBase { return output_; } - const vector& getSegmentationOutput() const - { - return segmentation_output_; - } - protected: void setOutput(const LidarData& output) { output_ = output; } - void setSegmentationOutput(vector& segmentation_output) - { - segmentation_output_ = segmentation_output; - } - private: LidarData output_; - vector segmentation_output_; }; }} //namespace diff --git a/AirLib/include/sensors/lidar/LidarSimple.hpp b/AirLib/include/sensors/lidar/LidarSimple.hpp index e837abf54a..db6f4b05a7 100644 --- a/AirLib/include/sensors/lidar/LidarSimple.hpp +++ b/AirLib/include/sensors/lidar/LidarSimple.hpp @@ -97,12 +97,12 @@ class LidarSimple : public LidarBase { LidarData output; output.point_cloud = point_cloud_; output.time_stamp = clock()->nowNanos(); - output.pose = lidar_pose; + output.pose = lidar_pose; + output.segmentation = segmentation_cloud_; last_time_ = output.time_stamp; setOutput(output); - setSegmentationOutput(segmentation_cloud_); } private: diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 25a19579dc..60f67d5c5b 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/AirLib/src/api/RpcLibClientBase.cpp @@ -189,11 +189,6 @@ msr::airlib::DistanceSensorData RpcLibClientBase::getDistanceSensorData(const st return pimpl_->client.call("getDistanceSensorData", distance_sensor_name, vehicle_name).as().to(); } -vector RpcLibClientBase::simGetLidarSegmentation(const std::string& lidar_name, const std::string& vehicle_name) const -{ - return pimpl_->client.call("simGetLidarSegmentation", lidar_name, vehicle_name).as>(); -} - bool RpcLibClientBase::simSetSegmentationObjectID(const std::string& mesh_name, int object_id, bool is_name_regex) { return pimpl_->client.call("simSetSegmentationObjectID", mesh_name, object_id, is_name_regex).as(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 2b41966c39..130c409e21 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/AirLib/src/api/RpcLibServerBase.cpp @@ -167,11 +167,6 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string& getVehicleSimApi(vehicle_name)->setTraceLine(color_rgba, thickness); }); - pimpl_->server. - bind("simGetLidarSegmentation", [&](const std::string& lidar_name, const std::string& vehicle_name) -> std::vector { - return getVehicleApi(vehicle_name)->getLidarSegmentation(lidar_name); - }); - pimpl_->server. bind("simSetSegmentationObjectID", [&](const std::string& mesh_name, int object_id, bool is_name_regex) -> bool { return getWorldSimApi()->setSegmentationObjectID(mesh_name, object_id, is_name_regex); From 649820250e68b61c10d5ce52cdcd3520c172effd Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 29 Jun 2020 15:08:06 +0530 Subject: [PATCH 2/4] [Pythonclient] Deprecate Lidar Segmentation API --- PythonClient/airsim/client.py | 4 +++- PythonClient/airsim/types.py | 1 + 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 27e6cc84b7..54c8a26b26 100644 --- a/PythonClient/airsim/client.py +++ b/PythonClient/airsim/client.py @@ -669,6 +669,7 @@ def getLidarData(self, lidar_name = '', vehicle_name = ''): def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): """ + NOTE: Deprecated API, use `getLidarData()` API instead Returns Segmentation ID of each point's collided object in the last Lidar update Args: @@ -678,7 +679,8 @@ def simGetLidarSegmentation(self, lidar_name = '', vehicle_name = ''): Returns: list[int]: Segmentation IDs of the objects """ - return self.client.call('simGetLidarSegmentation', lidar_name, vehicle_name) + logging.warning("simGetLidarSegmentation API is deprecated, use getLidarData() API instead") + return self.getLidarData(lidar_name, vehicle_name).segmentation # Plotting APIs def simFlushPersistentMarkers(self): diff --git a/PythonClient/airsim/types.py b/PythonClient/airsim/types.py index 021dc6dd88..063c801f1f 100644 --- a/PythonClient/airsim/types.py +++ b/PythonClient/airsim/types.py @@ -371,6 +371,7 @@ class LidarData(MsgpackMixin): point_cloud = 0.0 time_stamp = np.uint64(0) pose = Pose() + segmentation = 0 class ImuData(MsgpackMixin): time_stamp = np.uint64(0) From c70674c7ffdd5eb83af0fffef0f7864a46cde222 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Sat, 27 Jun 2020 09:19:40 +0530 Subject: [PATCH 3/4] [docs] Update Lidar documentation --- docs/lidar.md | 64 +++++++++++++++++++++++++++++---------------------- 1 file changed, 37 insertions(+), 27 deletions(-) diff --git a/docs/lidar.md b/docs/lidar.md index 1c592d5787..782c6a4323 100644 --- a/docs/lidar.md +++ b/docs/lidar.md @@ -1,17 +1,20 @@ # How to Use Lidar in AirSim -AirSim supports Lidar for multirotors and cars. +AirSim supports Lidar for multirotors and cars. The enablement of lidar and the other lidar settings can be configured via AirSimSettings json. Please see [general sensors](sensors.md) for information on configruation of general/shared sensor settings. ## Enabling lidar on a vehicle * By default, lidars are not enabled. To enable lidar, set the SensorType and Enabled attributes in settings json. + +```json + "Lidar1": { + "SensorType": 6, + "Enabled" : true, + } ``` - "Lidar1": { - "SensorType": 6, - "Enabled" : true, -``` + * Multiple lidars can be enabled on a vehicle. ## Lidar configuration @@ -27,12 +30,13 @@ HorizontalFOVStart | Horizontal FOV start for the lidar, in degrees HorizontalFOVEnd | Horizontal FOV end for the lidar, in degrees VerticalFOVUpper | Vertical FOV upper limit for the lidar, in degrees VerticalFOVLower | Vertical FOV lower limit for the lidar, in degrees -X Y Z | Position of the lidar relative to the vehicle (in NED, in meters) +X Y Z | Position of the lidar relative to the vehicle (in NED, in meters) Roll Pitch Yaw | Orientation of the lidar relative to the vehicle (in degrees, yaw-pitch-roll order to front vector +X) DataFrame | Frame for the points in output ("VehicleInertialFrame" or "SensorLocalFrame") -e.g., -``` +e.g. + +```json { "SeeDocsAt": "https://github.com/Microsoft/AirSim/blob/master/docs/settings_json.md", "SettingsVersion": 1.2, @@ -44,7 +48,7 @@ e.g., "VehicleType": "simpleflight", "AutoCreate": true, "Sensors": { - "LidarSensor1": { + "LidarSensor1": { "SensorType": 6, "Enabled" : true, "NumberOfChannels": 16, @@ -59,7 +63,7 @@ e.g., "DrawDebugPoints": true, "DataFrame": "SensorLocalFrame" }, - "LidarSensor2": { + "LidarSensor2": { "SensorType": 6, "Enabled" : true, "NumberOfChannels": 4, @@ -79,30 +83,36 @@ e.g., ``` ## Server side visualization for debugging -Be default, the lidar points are not drawn on the viewport. To enable the drawing of hit laser points on the viewport, please enable setting 'DrawDebugPoints' via settings json. -e.g., -``` - "Lidar1": { - ... - "DrawDebugPoints": true - }, + +By default, the lidar points are not drawn on the viewport. To enable the drawing of hit laser points on the viewport, please enable setting `DrawDebugPoints` via settings json. + +```json + "Lidar1": { + ... + "DrawDebugPoints": true + }, ``` -## Client API -Use `getLidarData()` API to retrieve the Lidar data. +**Note:** Enabling `DrawDebugPoints` can cause excessive memory usage and crash in releases `v1.3.1`, `v1.3.0`. This has been fixed in master and should work in later releases + +## Client API + +Use `getLidarData()` API to retrieve the Lidar data. + * The API returns a Point-Cloud as a flat array of floats along with the timestamp of the capture and lidar pose. -* Point-Cloud: - * The floats represent [x,y,z] coordinate for each point hit within the range in the last scan. - * The frame for the points in the output is configurable using "DataFrame" attribute - "" or "VehicleInertialFrame" -- default; returned points are in vehicle inertial frame (in NED, in meters) - "SensorLocalFrame" -- returned points are in lidar local frame (in NED, in meters) +* Point-Cloud: + * The floats represent [x,y,z] coordinate for each point hit within the range in the last scan. + * The frame for the points in the output is configurable using "DataFrame" attribute - + * "" or `VehicleInertialFrame` -- default; returned points are in vehicle inertial frame (in NED, in meters) + * `SensorLocalFrame` -- returned points are in lidar local frame (in NED, in meters) * Lidar Pose: * Lidar pose in the vehicle inertial frame (in NED, in meters) * Can be used to transform points to other frames. +* Segmentation: The segmentation of each lidar point's collided object ### Python Examples -- [drone_lidar.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//multirotor) -- [car_lidar.py](https://github.com/Microsoft/AirSim/tree/master/PythonClient//car) +- [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) ## Coming soon -* Visualization of lidar data on client side. \ No newline at end of file +* Visualization of lidar data on client side. From 9022946c7697e8fe2773383229a9503bfc1befb4 Mon Sep 17 00:00:00 2001 From: Rajat Singhal Date: Mon, 29 Jun 2020 16:28:41 +0530 Subject: [PATCH 4/4] Use generic findSensorByName function internally in VehicleApiBase --- AirLib/include/api/VehicleApiBase.hpp | 89 +++++---------------------- 1 file changed, 16 insertions(+), 73 deletions(-) diff --git a/AirLib/include/api/VehicleApiBase.hpp b/AirLib/include/api/VehicleApiBase.hpp index b9d1608ff8..a5c222207a 100644 --- a/AirLib/include/api/VehicleApiBase.hpp +++ b/AirLib/include/api/VehicleApiBase.hpp @@ -112,7 +112,7 @@ class VehicleApiBase : public UpdatableObject { // Lidar APIs virtual LidarData getLidarData(const std::string& lidar_name) const { - auto *lidar = findLidarByName(lidar_name); + auto *lidar = static_cast(findSensorByName(lidar_name, SensorBase::SensorType::Lidar)); if (lidar == nullptr) throw VehicleControllerException(Utils::stringf("No lidar with name %s exist on vehicle", lidar_name.c_str())); @@ -122,20 +122,7 @@ class VehicleApiBase : public UpdatableObject { // IMU API virtual ImuBase::Output getImuData(const std::string& imu_name) const { - const ImuBase* imu = nullptr; - - // Find imu with the given name (for empty input name, return the first one found) - // Not efficient but should suffice given small number of imus - uint count_imus = getSensors().size(SensorBase::SensorType::Imu); - for (uint i = 0; i < count_imus; i++) - { - const ImuBase* current_imu = static_cast(getSensors().getByType(SensorBase::SensorType::Imu, i)); - if (current_imu != nullptr && (current_imu->getName() == imu_name || imu_name == "")) - { - imu = current_imu; - break; - } - } + auto *imu = static_cast(findSensorByName(imu_name, SensorBase::SensorType::Imu)); if (imu == nullptr) throw VehicleControllerException(Utils::stringf("No IMU with name %s exist on vehicle", imu_name.c_str())); @@ -145,18 +132,7 @@ class VehicleApiBase : public UpdatableObject { // Barometer API virtual BarometerBase::Output getBarometerData(const std::string& barometer_name) const { - const BarometerBase* barometer = nullptr; - - uint count_barometers = getSensors().size(SensorBase::SensorType::Barometer); - for (uint i = 0; i < count_barometers; i++) - { - const BarometerBase* current_barometer = static_cast(getSensors().getByType(SensorBase::SensorType::Barometer, i)); - if (current_barometer != nullptr && (current_barometer->getName() == barometer_name || barometer_name == "")) - { - barometer = current_barometer; - break; - } - } + auto *barometer = static_cast(findSensorByName(barometer_name, SensorBase::SensorType::Barometer)); if (barometer == nullptr) throw VehicleControllerException(Utils::stringf("No barometer with name %s exist on vehicle", barometer_name.c_str())); @@ -166,18 +142,7 @@ class VehicleApiBase : public UpdatableObject { // Magnetometer API virtual MagnetometerBase::Output getMagnetometerData(const std::string& magnetometer_name) const { - const MagnetometerBase* magnetometer = nullptr; - - uint count_magnetometers = getSensors().size(SensorBase::SensorType::Magnetometer); - for (uint i = 0; i < count_magnetometers; i++) - { - const MagnetometerBase* current_magnetometer = static_cast(getSensors().getByType(SensorBase::SensorType::Magnetometer, i)); - if (current_magnetometer != nullptr && (current_magnetometer->getName() == magnetometer_name || magnetometer_name == "")) - { - magnetometer = current_magnetometer; - break; - } - } + auto *magnetometer = static_cast(findSensorByName(magnetometer_name, SensorBase::SensorType::Magnetometer)); if (magnetometer == nullptr) throw VehicleControllerException(Utils::stringf("No magnetometer with name %s exist on vehicle", magnetometer_name.c_str())); @@ -187,18 +152,7 @@ class VehicleApiBase : public UpdatableObject { // Gps API virtual GpsBase::Output getGpsData(const std::string& gps_name) const { - const GpsBase* gps = nullptr; - - uint count_gps = getSensors().size(SensorBase::SensorType::Gps); - for (uint i = 0; i < count_gps; i++) - { - const GpsBase* current_gps = static_cast(getSensors().getByType(SensorBase::SensorType::Gps, i)); - if (current_gps != nullptr && (current_gps->getName() == gps_name || gps_name == "")) - { - gps = current_gps; - break; - } - } + auto *gps = static_cast(findSensorByName(gps_name, SensorBase::SensorType::Gps)); if (gps == nullptr) throw VehicleControllerException(Utils::stringf("No gps with name %s exist on vehicle", gps_name.c_str())); @@ -208,18 +162,7 @@ class VehicleApiBase : public UpdatableObject { // Distance Sensor API virtual DistanceSensorData getDistanceSensorData(const std::string& distance_sensor_name) const { - const DistanceBase* distance_sensor = nullptr; - - uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance); - for (uint i = 0; i < count_distance_sensors; i++) - { - const DistanceBase* current_distance_sensor = static_cast(getSensors().getByType(SensorBase::SensorType::Distance, i)); - if (current_distance_sensor != nullptr && (current_distance_sensor->getName() == distance_sensor_name || distance_sensor_name == "")) - { - distance_sensor = current_distance_sensor; - break; - } - } + auto *distance_sensor = static_cast(findSensorByName(distance_sensor_name, SensorBase::SensorType::Distance)); if (distance_sensor == nullptr) throw VehicleControllerException(Utils::stringf("No distance sensor with name %s exist on vehicle", distance_sensor_name.c_str())); @@ -251,24 +194,24 @@ class VehicleApiBase : public UpdatableObject { }; private: - const LidarBase* findLidarByName(const std::string& lidar_name) const + const SensorBase* findSensorByName(const std::string& sensor_name, const SensorBase::SensorType type) const { - const LidarBase* lidar = nullptr; + const SensorBase* sensor = nullptr; - // Find lidar with the given name (for empty input name, return the first one found) - // Not efficient but should suffice given small number of lidars - uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar); - for (uint i = 0; i < count_lidars; i++) + // Find sensor with the given name (for empty input name, return the first one found) + // Not efficient but should suffice given small number of sensors + uint count_sensors = getSensors().size(type); + for (uint i = 0; i < count_sensors; i++) { - const LidarBase* current_lidar = static_cast(getSensors().getByType(SensorBase::SensorType::Lidar, i)); - if (current_lidar != nullptr && (current_lidar->getName() == lidar_name || lidar_name == "")) + const SensorBase* current_sensor = getSensors().getByType(type, i); + if (current_sensor != nullptr && (current_sensor->getName() == sensor_name || sensor_name == "")) { - lidar = current_lidar; + sensor = current_sensor; break; } } - return lidar; + return sensor; } };