[gazebo] Simulate Bebop1 camera (#2225)

* First attempt at bebop fisheye camera

* Try setting raw camera output size during initialization, does not work

SetImageSize() is ignored and SetEnvTextureSize() causes an
exception. Currently the camera output is too large for real-time
operation.

* FIX apply MT9F002_OUTPUT_SCALER to gazebo front camera

* Add MT9F002 camera cropping to fdm_gazebo

* Apply MT9F002_TARGET_FPS to bebop front cam in gazebo

* Set up bottom camera for bebop model

* Minor fixes and cleanup

* Fix MT9F002 partially applied to all cameras

* Remove sensors_params_bebop, introduce NPS_SIMULATE_MT9F002

Reduces duplicate code for sensor parameters, and follows the
same pattern as NPS_SIMULATE_LASER_RANGE_ARRAY.
Also does not require the sensor params to be changed in every
bebop airframe.

conf/airframes/tudelft/bebop_orangeavoid_course2017.xml

@@ -7,6 +7,9 @@
 
   <firmware name="rotorcraft">
     <target name="ap" board="bebop"/>
+    <target name="nps" board="pc">
+      <module name="fdm" type="gazebo"/>
+    </target>
 
     <!-- Front Camera parameters -->
     <define name="MT9F002_INITIAL_OFFSET_X" value="0.09" /> <!-- Offset from center position [-0.5..0.5]. Set to 0.09 to center horizon. -->
@@ -28,9 +31,7 @@
     <module name="actuators" type="bebop"/>
     <module name="imu" type="bebop"/>
     <module name="gps" type="datalink"/>
-    <module name="stabilization" type="indi_simple">
+    <module name="stabilization" type="indi_simple"/>
-      <define name="INDI_RPM_FEEDBACK" value="TRUE" />
-    </module>
     <module name="stabilization" type="rate_indi"/>
     <module name="ahrs" type="int_cmpl_quat">
       <configure name="USE_MAGNETOMETER" value="FALSE"/>
@@ -124,6 +125,10 @@
     <define name="BODY_TO_IMU_PHI" value="0." unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
     <define name="BODY_TO_IMU_PSI" value="0." unit="deg"/>
+    <!-- Magneto calibration -->
+    <define name="MAG_X_NEUTRAL" value="0"/>
+    <define name="MAG_Y_NEUTRAL" value="0"/>
+    <define name="MAG_Z_NEUTRAL" value="0"/>
   </section>
 
   <!-- local magnetic field -->
@@ -242,6 +247,7 @@
     <define name="JSBSIM_MODEL" value="simple_x_quad_ccw" type="string"/>
     <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
   </section>
+  <include href="conf/simulator/gazebo/airframes/bebop.xml"/>
 
   <section name="AUTOPILOT">
     <define name="MODE_STARTUP" value="AP_MODE_NAV"/>

conf/simulator/gazebo/airframes/ardrone2.xml

@@ -5,6 +5,5 @@
     <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/>
     <define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/>
     <define name="BYPASS_AHRS" value="1"/>
-    <define name="SIMULATE_VIDEO" value="1"/>
   </section>
 </airframe>

conf/simulator/gazebo/airframes/bebop.xml

@@ -8,5 +8,6 @@
     <define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>
     <define name="GAZEBO_AC_NAME" value="bebop" type="string"/>
     <define name="BYPASS_AHRS" value="1"/>
+    <define name="SIMULATE_MT9F002" value="1"/>
   </section>
 </airframe>

conf/simulator/gazebo/models/bebop/bebop.sdf

@@ -176,7 +176,7 @@
     <!-- CAMERAS -->
 
     <link name="front_camera">
-      <pose>0.15 0 0 0 0 0</pose>
+      <pose>0.15 0 0 -1.57 0.33 0</pose><!-- Bebop camera output is rotated 90 degrees and pitched slightly downwards -->
       <inertial>
         <mass>0.001</mass>
         <inertia>
@@ -188,17 +188,22 @@
           <ixz>0</ixz>
         </inertia>
       </inertial>
-      <sensor type="camera" name="front_camera">
+      <sensor type="wideanglecamera" name="front_camera">
-        <update_rate>30.0</update_rate>
+        <update_rate>15.0</update_rate><!-- adjust with MT9F002_TARGET_FPS -->
         <camera name="front_camera">
-          <horizontal_fov>1.3962634</horizontal_fov>
           <image>
-            <width>1280</width>
+            <width>4608</width><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS -->
-            <height>720</height>
+            <height>3288</height>
             <format>R8G8B8</format>
           </image>
+          <horizontal_fov>3.69</horizontal_fov>
+          <lens>
+            <type>equisolid_angle</type>
+            <scale_to_hfov>true</scale_to_hfov>
+            <env_texture_size>2048</env_texture_size><!-- with MT9F002_OUTPUT_SCALER = 1.00, will be scaled by NPS -->
+          </lens>
           <clip>
-            <near>0.02</near>
+            <near>0.01</near>
             <far>300</far>
           </clip>
           <noise>
@@ -219,7 +224,7 @@
     </joint>
     
     <link name="bottom_camera">
-      <pose>0 0 -.03 0 1.57 0</pose>
+      <pose>0 0 0 0 1.57 0</pose>
       <inertial>
         <mass>0.001</mass>
         <inertia>
@@ -234,9 +239,9 @@
       <sensor type="camera" name="bottom_camera">
         <update_rate>30.0</update_rate>
         <camera name="bottom_camera">
-          <horizontal_fov>1.3962634</horizontal_fov>
+          <horizontal_fov>0.7175</horizontal_fov>
           <image>
-            <width>320</width>
+            <width>240</width>
             <height>240</height>
             <format>R8G8B8</format>
           </image>

sw/simulator/nps/nps_fdm_gazebo.cpp

@@ -98,7 +98,10 @@ struct gazebocam_t {
 };
 static struct gazebocam_t gazebo_cams[VIDEO_THREAD_MAX_CAMERAS] =
 { { NULL, 0 } };
+#if NPS_SIMULATE_MT9F002
+#include "boards/bebop/mt9f002.h"
 #endif
+#endif // NPS_SIMULATE_VIDEO
 
 struct gazebo_actuators_t {
   string names[NPS_COMMANDS_NB];
@@ -344,7 +347,8 @@ static void init_gazebo(void)
   sdf::init(vehicle_sdf);
   sdf::readFile(gazebodir + "models/" + NPS_GAZEBO_AC_NAME + "/" + NPS_GAZEBO_AC_NAME + ".sdf", vehicle_sdf);
 
-  // add sensors
+  // add or set up sensors before the vehicle gets loaded
+  // laser range array
 #if NPS_SIMULATE_LASER_RANGE_ARRAY
   vehicle_sdf->Root()->GetFirstElement()->AddElement("include")->GetElement("uri")->Set("model://range_sensors");
   sdf::ElementPtr range_joint = vehicle_sdf->Root()->GetFirstElement()->AddElement("joint");
@@ -353,6 +357,25 @@ static void init_gazebo(void)
   range_joint->GetElement("parent")->Set("chassis");
   range_joint->GetElement("child")->Set("range_sensors::base");
 #endif
+  // bebop front camera
+#ifdef NPS_SIMULATE_MT9F002
+  sdf::ElementPtr link = vehicle_sdf->Root()->GetFirstElement()->GetElement("link");
+  while(link) {
+    if(link->Get<string>("name") == "front_camera") {
+      int w = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Get<int>();
+      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("width")->Set(w * MT9F002_OUTPUT_SCALER);
+      int h = link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Get<int>();
+      link->GetElement("sensor")->GetElement("camera")->GetElement("image")->GetElement("height")->Set(h * MT9F002_OUTPUT_SCALER);
+      int env = link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Get<int>();
+      link->GetElement("sensor")->GetElement("camera")->GetElement("lens")->GetElement("env_texture_size")->Set(env * MT9F002_OUTPUT_SCALER);
+      cout << "Applied MT9F002_OUTPUT_SCALER (=" << MT9F002_OUTPUT_SCALER << ") to " << link->Get<string>("name") << endl;
+      link->GetElement("sensor")->GetElement("update_rate")->Set(MT9F002_TARGET_FPS);
+      cout << "Applied MT9F002_TARGET_FPS (=" << MT9F002_TARGET_FPS << ") to " << link->Get<string>("name") << endl;
+    }
+    link = link->GetNextElement("link");
+  }
+#endif // NPS_SIMULATE_MT9F002
+
 
   // get world
   cout << "Load world: " << gazebodir + "world/" + NPS_GAZEBO_WORLD << endl;
@@ -644,6 +667,17 @@ static void init_gazebo_video(void)
     cameras[i]->sensor_size.h = cam->ImageHeight();
     cameras[i]->crop.w = cam->ImageWidth();
     cameras[i]->crop.h = cam->ImageHeight();
+#if NPS_SIMULATE_MT9F002
+    // See boards/bebop/mt9f002.c
+    if(cam->Name() == "front_camera") {
+      cameras[i]->output_size.w = MT9F002_OUTPUT_WIDTH;
+      cameras[i]->output_size.h = MT9F002_OUTPUT_HEIGHT;
+      cameras[i]->sensor_size.w = MT9F002_OUTPUT_WIDTH;
+      cameras[i]->sensor_size.h = MT9F002_OUTPUT_HEIGHT;
+      cameras[i]->crop.w = MT9F002_OUTPUT_WIDTH;
+      cameras[i]->crop.h = MT9F002_OUTPUT_HEIGHT;
+    }
+#endif
     cameras[i]->fps = cam->UpdateRate();
     cout << "ok" << endl;
   }
@@ -701,13 +735,25 @@ static void read_image(
   struct image_t *img,
   gazebo::sensors::CameraSensorPtr cam)
 {
+  int xstart = 0;
+  int ystart = 0;
+#if NPS_SIMULATE_MT9F002
+  if(cam->Name() == "front_camera") {
+    image_create(img, MT9F002_OUTPUT_WIDTH, MT9F002_OUTPUT_HEIGHT, IMAGE_YUV422);
+    xstart = cam->ImageWidth() * (0.5 + MT9F002_INITIAL_OFFSET_X) - MT9F002_OUTPUT_WIDTH / 2;
+    ystart = cam->ImageHeight() * (0.5 + MT9F002_INITIAL_OFFSET_Y) - MT9F002_OUTPUT_HEIGHT / 2;
+  } else {
+    image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
+  }
+#else
   image_create(img, cam->ImageWidth(), cam->ImageHeight(), IMAGE_YUV422);
+#endif
   // Convert Gazebo's *RGB888* image to Paparazzi's YUV422
   const uint8_t *data_rgb = cam->ImageData();
   uint8_t *data_yuv = (uint8_t *)(img->buf);
   for (int x = 0; x < img->w; ++x) {
     for (int y = 0; y < img->h; ++y) {
-      int idx_rgb = 3 * (img->w * y + x);
+      int idx_rgb = 3 * (cam->ImageWidth() * (y + ystart) + (x + xstart));
       int idx_yuv = 2 * (img->w * y + x);
       int idx_px = img->w * y + x;
       if (idx_px % 2 == 0) { // Pick U or V