Update FlightGoggles to v2.0.3. Adds support for downward range finde…

…r (issue #52). * Makes camera resolution configurable via ROS param files. Alleviates issues from #51 and #32. * Lowers takeoff thrust threshold to 9.9N (see issues #56, #45). * Adds Windows64 standalone build to releases page (requested in issues #53, #46). * Adds nominal gate location file to ROS param server. * Adds plotter for laser range finder. * Adds example stereo reconstruction pipeline launch file. Many thanks to @eatal and @varunmurali1 for their help with this patch!
mit-aera · Feb 19, 2019 · 1ff625d · 1ff625d
1 parent 1781259
commit 1ff625d
Show file tree

Hide file tree

Showing 15 changed files with 265 additions and 22 deletions.
diff --git a/.gitignore b/.gitignore
@@ -138,3 +138,8 @@ tags
 .history
 
 # End of https://www.gitignore.io/api/vim,ros,c++,cmake,visualstudiocode
+
+# Ignore AlphaPilot Challenge files
+flightgoggles/config/challenges/
+flightgoggles/config/perturbations/
+flightgoggles/launch/scorer.*
diff --git a/README.md b/README.md
@@ -86,6 +86,8 @@ catkin build
 roslaunch flightgoggles teleopExample.launch
 # To run core simulation framework without teleoperation
 roslaunch flightgoggles core.launch
+# To run teleoperation with sample stereo pipeline
+roslaunch flightgoggles stereoTeleopExample.launch # WARNING! May crash! Read "Does FlightGoggles currently support Stereo?" FAQ for more info.
 ```
 
 **NOTE:** The FlightGoggles beta might take up to 30 seconds to load. In the development build, asset loading to the GPU has not yet been optimized. If you'd like to keep the FlightGoggles render alive between tests to avoid waiting for the renderer to load, you can run the following:
@@ -220,9 +222,16 @@ Your computer is likely assigned a local IP address behind a NAT. Please follow
 FlightGoggles is similar to the Gazebo simulator in that it can scale the ROS clock down if the simulation is running slowly. Thus, your autonomous algorithms will see a constant 60Hz camera in `sim time` and should experience accurate drone dynamics as long as the camera renderer is able to run ([see ROS clock documentation](http://wiki.ros.org/Clock)).   
 
 **Q. Does FlightGoggles currently support Stereo?**  
-Yes! Simply pass `render_stereo:=true` to the FlightGoggles launch file. **Note:** as of `v2.0.2` the combined vertical resolution of the two stereo cameras must not be greater than the vertical resolution of the largest monitor attached to your computer. In other words, you must have a monitor with `>=1536` vertical resolution to render stereo at the default camera resolution. A current workaround is to [decrease the default camera resolution](https://github.com/mit-fast/FlightGoggles/blob/master/flightgoggles_ros_bridge/src/Common/jsonMessageSpec.hpp#L65), attach a high resolution "dummy" monitor, or rotate or attach a vertical display to your rendering computer. 
+Yes! Simply run `roslaunch flightgoggles stereoTeleopExample.launch` or pass `render_stereo:=true` to any FlightGoggles launch file.
 
-As an explanation for the current restriction, FlightGoggles uses the GPU backbuffer for storing rendered images. However, the size of this buffer is limited by the screen resolution. Thus, concatenated images cannot be larger than the size of the GPU backbuffer. This issue will be patched in a later release.
+**Note:** as of `v2.0.3` the combined vertical resolution of the two stereo cameras must not be greater than the vertical resolution of the largest monitor attached to your computer. 
+In other words, you must have a monitor with `>=1536` vertical resolution to render stereo at the default camera resolution. 
+A current workaround is to [decrease the default camera resolution](https://github.com/mit-fast/FlightGoggles/blob/master/flightgoggles/config/drone/drone.yaml#L47),
+ rotate your display, or attach a vertical display to your rendering computer.
+
+As an explanation for the current restriction, FlightGoggles uses the GPU backbuffer for storing rendered images. 
+However, the size of this buffer is limited by the screen resolution. 
+Thus, concatenated images cannot be larger than the size of the GPU backbuffer. 
 
 **Q. Is there an easy way to reset the simulation to its intial state without having to restart FlightGoggles?**
 
@@ -239,6 +248,13 @@ So far, we have seen 2 reasons for crashes on startup:
     - Diagnosable by checking line in player.log similar to `Vulkan renderer=[GeForce GTX 750 Ti] id=[1380].` 
     - If the listed GPU is not your desired GPU, set the environment variable `VK_ICD_FILENAMES` to `/usr/share/vulkan/icd.d/nvidia_icd.json` as described [here](https://wiki.archlinux.org/index.php/Vulkan). See issue [#28](https://github.com/mit-fast/FlightGoggles/issues/28) for more detail.
 
+**Q. Why is the output from the downward facing laser rangefinder negative?**
+The laser rangefinder is pointed in the -Z direction of the `/uav/imu` frame. Thus, the output of the laser rangefinder is
+ always negative. 
+
+**Q. What is the max range of the downward facing laser rangefinder**
+The laser rangefinder's range is 20m. Any readings larger than 20m signify that no obstacles were detected within its 20m detection range. 
+
 ## Citation
 If you find this work useful for your research, please cite:
 ```bibtex

diff --git a/flightgoggles/config/challenges/nominal_gate_locations.yaml b/flightgoggles/config/challenges/nominal_gate_locations.yaml
@@ -0,0 +1,70 @@
+Gate1:
+  nominal_location: [[-0.009051204, -34.8755, 3.071861], [-0.008949876, -32.9505, 3.071861], [-0.008949876, -32.9505, 1.134362], [-0.009051204, -34.8755, 1.134362]]
+  perturbation_bound: [2.5, 2.5, 5.0]
+Gate13:
+  nominal_location: [[1.237332, 9.001728, 2.9625], [3.162332, 9.001728, 2.9625], [3.162332, 9.001728, 1.025], [1.237332, 9.001728, 1.025]]
+  perturbation_bound: [2.5, 2.5, 5.0]
+Gate3:
+  nominal_location: [[-0.3703381, -12.99827, 4.728], [-0.3699976, -11.45827, 4.728], [-0.3699976, -11.45827, 3.177999], [-0.3703381, -12.99827, 3.177999]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate4:
+  nominal_location: [[-31.56487, -13.929, 6.99714], [-30.04735, -13.929, 7.259325], [-29.78347, -13.929, 5.731955], [-31.30098, -13.929, 5.469769]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate5:
+  nominal_location: [[-20.94317, 14.607, 2.515013], [-20.94283, 16.147, 2.515013], [-20.94283, 16.147, 0.965013], [-20.94317, 14.607, 0.965013]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate7:
+  nominal_location: [[16.2506, 41.63, 11.32092], [17.79061, 41.63, 11.32092], [17.79061, 41.63, 9.770914], [16.2506, 41.63, 9.770914]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate8:
+  nominal_location: [[-3.430186, 11.79953, 6.122593], [-3.430186, 12.97997, 6.122593], [-3.430186, 11.79952, 4.911986], [-3.430186, 12.97997, 4.911986]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate9:
+  nominal_location: [[-6.40867, -12.13678, 4.152941], [-8.208672, -12.13678, 4.152941], [-6.408669, -12.13678, 2.306941], [-8.208672, -12.13678, 2.306941]]
+  perturbation_bound: [0.0, 0.0, 2.0]
+Gate10:
+  nominal_location: [[18.15111, 3.631447, 7.229498], [16.35111, 3.63155, 7.229498], [18.15111, 3.631447, 5.383497], [16.35111, 3.63155, 5.383497]]
+  perturbation_bound: [2.0, 2.0, 5.0]
+Gate11:
+  nominal_location: [[-27.01367, 2.543159, 7.515942], [-28.81367, 2.543289, 7.515942], [-27.01367, 2.543159, 5.669941], [-28.81367, 2.543289, 5.669941]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate12:
+  nominal_location: [[18.40823, 13.60605, 9.143385], [16.33437, 14.09245, 9.143507], [18.90706, 15.73291, 9.14324], [16.83319, 16.21931, 9.143364]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate23:
+  nominal_location: [[-9.328671, 7.773174, 2.942941], [-11.12867, 7.773277, 2.942941], [-9.328669, 7.773174, 1.096941], [-11.12867, 7.773277, 1.096941]]
+  perturbation_bound: [5.0, 5.0 ,5.0]
+Gate6:
+  nominal_location: [[-9.14867, 30.62316, 3.820941], [-10.94867, 30.62329, 3.820941], [-9.148668, 30.62316, 1.974941], [-10.94867, 30.62329, 1.974941]]
+  perturbation_bound: [0.0, 0.0, 2.0]
+Gate2:
+  nominal_location: [[0.8248205, 27.86797, 4.16025], [3.834821, 27.86797, 4.16025], [3.834821, 27.86797, 0.9327497], [0.8248205, 27.86797, 0.9327497]]
+  perturbation_bound: [0.0, 1.0, 5.0]
+Gate14:
+  nominal_location: [[-8.674376, -29.53623, 4.25625], [-6.545984, -27.40784, 4.25625], [-6.545984, -27.40784, 1.02875], [-8.674376, -29.53623, 1.02875]]
+  perturbation_bound: [1.0, 1.0, 5.0]
+Gate15:
+  nominal_location: [[5.744822, -11.79203, 4.16025], [8.75482, -11.79203, 4.16025], [8.75482, -11.79203, 0.9327497], [5.744822, -11.79203, 0.9327497]]
+  perturbation_bound: [0.0, 2.5, 5.0]
+Gate16:
+  nominal_location: [[-13.73218, 28.64197, 12.65825], [-13.73218, 26.23396, 12.65825], [-13.73218, 26.23396, 10.07625], [-13.73218, 28.64197, 10.07625]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate17:
+  nominal_location: [[-20.57, -13.29803, 3.46425], [-20.57, -15.70603, 3.46425], [-20.57, -15.70603, 0.8822498], [-20.57, -13.29803, 0.8822498]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate18:
+  nominal_location: [[13.51782, 21.76397, 3.61825], [13.51782, 19.35596, 3.61825], [13.51782, 19.35596, 1.03625], [13.51782, 21.76397, 1.03625]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate19:
+  nominal_location: [[15.96136, -17.26184, 12.31488], [18.36892, -17.26184, 12.36067], [18.41802, -17.26184, 9.779139], [16.01046, -17.26184, 9.733349]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate20:
+  nominal_location: [[-25.39118, 25.019, 7.856133], [-22.98318, 25.019, 7.856133], [-22.98318, 25.019, 5.274133], [-25.39118, 25.019, 5.274133]]
+  perturbation_bound: [0.0, 0.0, 0.0]
+Gate21:
+  nominal_location: [[15.68563, 38.50578, 8.376249], [17.81402, 40.63416, 8.376249], [17.81402, 40.63416, 5.14875], [15.68563, 38.50578, 5.14875]]
+  perturbation_bound: [0.0, 0.0, 2.0]
+Gate22:
+  nominal_location: [[4.803625, -27.37584, 4.18125], [6.932016, -29.50423, 4.18125], [6.932016, -29.50423, 0.9537499], [4.803625, -27.37584, 0.9537499]]
+  perturbation_bound: [1.0, 1.0, 5.0]
+
diff --git a/flightgoggles/config/drone/drone.yaml b/flightgoggles/config/drone/drone.yaml
@@ -15,11 +15,16 @@ flightgoggles_uav_dynamics:
     angular_process_noise: 0.00025
     linear_process_noise: 0.0005
     reset_timeout: 0.1
+    min_arming_thrust: 9.9
 
 flightgoggles_imu:
     accelerometer_variance: 0.005
     gyroscope_variance: 0.003
 
+flightgoggles_laser:
+    rangefinder_variance: 0.009
+    rangefinder_max_range: 20.0
+
 flightgoggles_lpf:
     gain_p: 35530.5758439217
     gain_q: 266.572976289502
@@ -37,3 +42,9 @@ flightgoggles_pid:
     int_bound_roll: 1000.0
     int_bound_pitch: 1000.0
     int_bound_yaw: 1000.0
+
+flightgoggles_ros_bridge:
+   image_width: 1024
+   image_height: 768
+   baseline: 0.32
+   collider_radius: 0.15
diff --git a/flightgoggles/config/stereo_camera.yaml b/flightgoggles/config/stereo_camera.yaml
diff --git a/flightgoggles/launch/core.launch b/flightgoggles/launch/core.launch
@@ -11,7 +11,8 @@
   <group ns="/uav">
 
     <!-- Read required yaml files" / --> 
-    <rosparam command="load" file="$(find flightgoggles)/config/drone/drone.yaml"/> 
+    <rosparam command="load" file="$(find flightgoggles)/config/drone/drone.yaml"/>
+    <rosparam command="load" file="$(find flightgoggles)/config/challenges/nominal_gate_locations.yaml"/>
 
     <!-- Environment Creator -->
     <!-- flightgoggles_uav_dynamics simulator -->
@@ -44,6 +45,9 @@
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find flightgoggles)/rviz/third_person_drone_follower.rviz" unless="$(arg render_stereo)"/>
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find flightgoggles)/rviz/third_person_drone_follower_stereo.rviz" if="$(arg render_stereo)"/>
 
+    <!--Run rqt_plot for plotting readings from the laser range finder-->
+    <node type="rqt_plot" name="laser_rangefinder_plotter" pkg="rqt_plot" args="topics /uav/sensors/downward_laser_rangefinder/range"/>
+
     <!--Static transforms for 15cm stereo baseline -->
     <node pkg="tf2_ros" type="static_transform_publisher"
           name="camera_left_link" args="0 0.0 0 -1.57079632679 0 -1.57079632679 uav/imu uav/camera/left  " unless="$(arg render_stereo)"/>

diff --git a/flightgoggles/launch/stereoTeleopExample.launch b/flightgoggles/launch/stereoTeleopExample.launch
@@ -0,0 +1,44 @@
+<launch>
+  <arg name="use_external_renderer" default="0" />
+  <arg name="ignore_collisions" default="false" />
+  <arg name="render_stereo" default="1" />
+
+  <group ns="/control_nodes">
+    <!-- Start human teleoperation nodes -->
+    <node name="joy" pkg="joy" type="joy_node"/>
+    <node name="keyboard" pkg="keyboard" type="keyboard"/>
+    <node name="universal_teleop" pkg="universal_teleop" type="universal_teleop" output="screen">
+      <rosparam file="$(find universal_teleop)/launch/example_input_map.yml"/>
+
+      <remap from="output/rateThrust" to="/uav/input/rateThrust"/>
+      <remap from="output/takeoff" to="/uav/input/takeoff"/>
+      <remap from="output/land" to="/uav/input/land"/>
+      <remap from="output/reset" to="/uav/input/reset"/>
+    </node>
+  </group>
+
+  <!--include core flightgoggles launch file-->
+  <include file="$(find flightgoggles)/launch/core.launch">
+    <arg name="use_external_renderer" value="$(arg use_external_renderer)"/>
+    <arg name="render_stereo" value="$(arg render_stereo)"/>
+    <arg name="ignore_collisions" value="$(arg ignore_collisions)"/>
+  </include>
+
+  <!-- Launch override settings. Must be placed at end of file to override params set in core launchfile. -->
+
+  <!--For teleoperation, run dynamics in real time-->
+  <param name="/use_sim_time" value="false" />
+
+  <group ns="/uav/camera/">
+     <node name="stereo_image_proc" type="stereo_image_proc" pkg="stereo_image_proc" output="screen">
+       <remap from="left/image_raw" to="left/image_rect_color"/>
+       <remap from="right/image_raw" to="right/image_rect_color"/>
+       <param name="min_disparity" value="10"/>
+       <param name="texture_threshold" value="1"/>
+       <param name="speckle_range" value="2"/>
+       <param name="uniqueness_ratio" value="10.0"/>
+       <param name="prefilter_cap" value="5.0"/>
+     </node>
+  </group>
+
+</launch>
diff --git a/flightgoggles/package.xml b/flightgoggles/package.xml
@@ -55,7 +55,8 @@
 
   <!--Optional, but useful for playing rosbags on repeat.-->
   <exec_depend>rosbaglive</exec_depend>
-
+  <!--For stereo-->
+  <exec_depend>stereo_image_proc</exec_depend>
 
   <!--<exec_depend>flightgoggles_uav_dynamics</exec_depend>-->
   <!--<exec_depend>flightgoggles_ros_bridge</exec_depend>-->

diff --git a/flightgoggles/renderer_version/FlightGoggles_Data.tar.gz.md5 b/flightgoggles/renderer_version/FlightGoggles_Data.tar.gz.md5
@@ -1 +1 @@
-78bba246d39b91e24152d4b5e8c13c0c
+1ca07cb660744ac6425c00f030b32df0
diff --git a/flightgoggles/rviz/third_person_drone_follower_stereo.rviz b/flightgoggles/rviz/third_person_drone_follower_stereo.rviz
@@ -9,8 +9,9 @@ Panels:
         - /Grid1
         - /TF1
         - /TF1/Frames1
+        - /PointCloud21
       Splitter Ratio: 0.5
-    Tree Height: 398
+    Tree Height: 373
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -106,6 +107,36 @@ Visualization Manager:
       Transport Hint: raw
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.100000001
+      Style: Flat Squares
+      Topic: /uav/camera/points2
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -131,7 +162,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/ThirdPersonFollower
-      Distance: 1.59713197
+      Distance: 17.2551746
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
@@ -146,10 +177,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
-      Pitch: 0.170202836
+      Pitch: -0.0197971538
       Target Frame: uav/imu
       Value: ThirdPersonFollower (rviz)
-      Yaw: 3.02386951
+      Yaw: 3.06886983
     Saved: ~
 Window Geometry:
   Displays:
@@ -159,7 +190,7 @@ Window Geometry:
   Hide Right Dock: true
   Image:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a0000021dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002a90000021d000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000305000003bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000003d5000003bf000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006980000027bfc0100000004fb0000000a0049006d00610067006501000000000000038b0000005c00fffffffb0000000a0049006d0061006700650100000391000003070000005c00fffffffb0000000a0049006d0061006700650100000000000006980000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000069800000082fc0100000002fc00000000000006980000030000fffffffa000000010200000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000800540069006d0065010000052c0000003e0000003b00fffffffb0000000800540069006d00650100000000000004500000000000000000000005280000021d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000204fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000002c200000204000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c000002610000000100000305000003bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000003d5000003bf000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b2000000000000000000000002000006980000027bfc0100000004fb0000000a0049006d00610067006501000000000000038b0000005c00fffffffb0000000a0049006d0061006700650100000391000003070000005c00fffffffb0000000a0049006d0061006700650100000000000006980000000000000000fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000069800000082fc0100000002fc00000000000006980000030000fffffffa000000010200000002fb0000000a0049006d0061006700650000000000ffffffff0000000000000000fb0000000800540069006d0065010000052c0000003e0000003b00fffffffb0000000800540069006d00650100000000000004500000000000000000000005280000020400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:

diff --git a/flightgoggles_ros_bridge/src/Common/jsonMessageSpec.hpp b/flightgoggles_ros_bridge/src/Common/jsonMessageSpec.hpp
@@ -148,6 +148,7 @@ struct RenderMetadata_t
   // Status update from collision detectors and raycasters.
   bool hasCameraCollision;
   std::vector<Landmark_t> landmarksInView;
+  float lidarReturn;
 };
 
 // Json Parsers
@@ -170,6 +171,7 @@ inline void from_json(const json &j, RenderMetadata_t &o)
   o.channels = j.at("channels").get<std::vector<int>>();
   o.hasCameraCollision = j.at("hasCameraCollision").get<bool>();
   o.landmarksInView = j.at("landmarksInView").get<std::vector<Landmark_t>>();
+  o.lidarReturn = j.at("lidarReturn").get<float>();
 
 }
Original file line number	Diff line number	Diff line change
		@@ -1 +1 @@
		78bba246d39b91e24152d4b5e8c13c0c
		1ca07cb660744ac6425c00f030b32df0