Simulating lidar in foggy environment (#725)

PX4 · Mar 9, 2021 · 6b6f474 · 6b6f474
1 parent 26fb250
commit 6b6f474
Show file tree

Hide file tree

Showing 6 changed files with 138 additions and 1 deletion.
diff --git a/include/gazebo_lidar_plugin.h b/include/gazebo_lidar_plugin.h
@@ -75,6 +75,7 @@ namespace gazebo
       double max_distance_;
       double low_signal_strength_;
       double high_signal_strength_;
+      bool simulate_fog_;
 
       gazebo::msgs::Quaternion orientation_;
 

diff --git a/models/foggy_lidar/foggy_lidar.sdf b/models/foggy_lidar/foggy_lidar.sdf
@@ -0,0 +1,64 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <model name="foggy_lidar">
+    <link name="link">
+
+      <inertial>
+        <pose>0 0 0 0 0 0</pose>
+        <mass>0.19</mass>
+        <inertia>
+          <ixx>4.15e-6</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>2.407e-6</iyy>
+          <iyz>0</iyz>
+          <izz>2.407e-6</izz>
+        </inertia>
+      </inertial>
+
+      <visual name="visual">
+        <geometry>
+          <box>
+            <size>0.02 0.05 0.05</size>
+          </box>
+        </geometry>
+      </visual>
+
+      <sensor name="laser" type="ray">
+        <pose>0 0 0 0 1.51 0</pose>
+        <always_on>1</always_on>
+        <update_rate>5.5</update_rate>
+        <visualize>true</visualize>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>360</samples>
+              <resolution>1</resolution>
+              <min_angle>-3.14</min_angle>
+              <max_angle>3.14</max_angle>
+            </horizontal>
+          </scan>
+          <range>
+            <min>0.06</min>
+            <max>120</max>
+            <resolution>0.05</resolution>
+          </range>
+          <noise>
+            <type>gaussian</type>
+            <mean>0.0</mean>
+            <stddev>1.0</stddev>
+          </noise>
+        </ray>
+        <plugin name="LaserPlugin"
+          filename="libgazebo_lidar_plugin.so">
+          <robotNamespace></robotNamespace>
+          <min_distance>0.1</min_distance>
+          <max_distance>100.0</max_distance>
+          <simulate_fog>true</simulate_fog>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+</sdf>
+
+<!-- vim: set et fenc= ff=unix sts=0 sw=2 ts=2 : -->
diff --git a/models/foggy_lidar/model.config b/models/foggy_lidar/model.config
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>foggy_lidar</name>
+  <version>1.0</version>
+  <sdf>foggy_lidar.sdf</sdf>
+
+  <author>
+    <name>David Jablonski</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+		This lidar simulates a foggy environment with an average distance measurement of 2m and a standard deviation of 1.
+  </description>
+</model>
+
+<!-- vim: set noet fenc= ff=unix sts=0 sw=2 ts=2 : -->
diff --git a/models/iris_foggy_lidar/iris_foggy_lidar.sdf b/models/iris_foggy_lidar/iris_foggy_lidar.sdf
@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+<sdf version='1.5'>
+  <model name='iris_foggy_lidar'>
+
+    <include>
+      <uri>model://iris</uri>
+    </include> 
+
+    <include>
+      <uri>model://foggy_lidar</uri>
+      <pose>0 0 0.1 0 1.571 0</pose>
+    </include>
+    <joint name="foggy_lidar_joint" type="revolute">
+      <parent>iris::base_link</parent>
+      <child>foggy_lidar::link</child>
+      <pose>0 0 0.1 0 1.571 0</pose>
+      <axis>
+        <xyz>0 0 1</xyz>
+        <limit>
+          <upper>0</upper>
+          <lower>0</lower>
+        </limit>
+      </axis>
+    </joint>
+
+  </model>
+</sdf>
+<!-- vim: set noet fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
+
diff --git a/models/iris_foggy_lidar/model.config b/models/iris_foggy_lidar/model.config
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>3DR Iris with a foggy lidar measurement</name>
+  <version>1.0</version>
+  <sdf>iris_foggy_lidar.sdf</sdf>
+
+  <author>
+   <name>David Jablonski</name>
+   <email>[email protected]</email>
+  </author>
+
+  <description>
+    This is a model of the 3DR Iris Quadrotor with a foggy lidar measurement.
+  </description>
+</model>
diff --git a/src/gazebo_lidar_plugin.cpp b/src/gazebo_lidar_plugin.cpp
@@ -28,6 +28,7 @@
 #include <stdio.h>
 #include <boost/algorithm/string.hpp>
 #include <common.h>
+#include <ignition/math/Rand.hh>
 
 using namespace gazebo;
 using namespace std;
@@ -68,6 +69,11 @@ void LidarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
   else
     gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n";
 
+  if (_sdf->HasElement("simulate_fog")) {
+    simulate_fog_ = _sdf->GetElement("simulate_fog")->Get<bool>();
+  } else {
+    simulate_fog_ = false;
+  }
   // get minimum distance
   if (_sdf->HasElement("min_distance")) {
     min_distance_ = _sdf->GetElement("min_distance")->Get<double>();
@@ -152,12 +158,16 @@ void LidarPlugin::OnNewLaserScans()
   double current_distance = parentSensor_->Range(0);
 
   // set distance to min/max if actual value is smaller/bigger
-  if (current_distance < min_distance_ || std::isinf(current_distance)) {
+  if (simulate_fog_ && current_distance > 2.0f) {
+    double whiteNoise = ignition::math::Rand::DblNormal(0.0f, 0.1f);
+    current_distance = 2.0f + whiteNoise;
+  } else if (current_distance < min_distance_ || std::isinf(current_distance)) {
     current_distance = min_distance_;
   } else if (current_distance > max_distance_) {
     current_distance = max_distance_;
   }
 
+
   lidar_message_.set_current_distance(current_distance);
   lidar_message_.set_h_fov(kDefaultFOV);
   lidar_message_.set_v_fov(kDefaultFOV);