Fix crash with DEM and Camera (#3279)

* Test DEM heightmap with camera. This exposes a crash.
* rendering: ensure SphericalCoordinates != nullptr

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

gazebo/rendering/Heightmap.cc

@@ -425,6 +425,11 @@ void Heightmap::Load()
        sphericalCoordinates = boost::make_shared<common::SphericalCoordinates>(
                   common::SphericalCoordinates::MOON_SCS);
     }
+    else
+    {
+       sphericalCoordinates = boost::make_shared<common::SphericalCoordinates>(
+                  common::SphericalCoordinates::EARTH_WGS84);
+    }
 
     this->dataPtr->heightmapData = common::HeightmapDataLoader::LoadTerrainFile(
         this->dataPtr->filename, sphericalCoordinates);

test/integration/dem.cc

@@ -25,6 +25,20 @@
 using namespace gazebo;
 class Dem_TEST : public ServerFixture
 {
+  /////////////////////////////////////////////////
+  public: void OnNewCameraFrame(int* _imageCounter, unsigned char* _imageDest,
+                    const unsigned char *_image,
+                    unsigned int _width, unsigned int _height,
+                    unsigned int _depth,
+                    const std::string &/*_format*/)
+  {
+    std::lock_guard<std::mutex> lock(this->mutex);
+    memcpy(_imageDest, _image, _width * _height * _depth);
+    *_imageCounter += 1;
+  }
+
+  public: unsigned char* img = NULL;
+  private: std::mutex mutex;
 };
 
 #ifdef HAVE_GDAL
@@ -71,6 +85,75 @@ TEST_F(Dem_TEST, GPS)
   EXPECT_NEAR(sensor->Altitude(),
       elevation + model->WorldPose().Pos().Z(), 1);
 }
+
+/////////////////////////////////////////////////
+/// \brief Test the integration between a camera and a DEM terrain.
+TEST_F(Dem_TEST, Camera)
+{
+  // Load a DEM world with a GPS sensor (without noise) attached to a box.
+  Load("worlds/heightmap_dem.world");
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_TRUE(world != NULL);
+
+  // spawn camera sensor to capture an image of heightmap
+  std::string modelName = "camera_model";
+  std::string cameraName = "camera_sensor";
+  unsigned int width  = 320;
+  unsigned int height = 240;
+  double updateRate = 10;
+  ignition::math::Pose3d testPose(
+      ignition::math::Vector3d(0, 0, 100),
+      ignition::math::Quaterniond(0, 1.57, 0));
+  SpawnCamera(modelName, cameraName, testPose.Pos(),
+      testPose.Rot().Euler(), width, height, updateRate);
+
+  // Get a pointer to the CameraSensor.
+  sensors::SensorPtr sensor = sensors::get_sensor(cameraName);
+  sensors::CameraSensorPtr camSensor =
+    std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
+
+  // Make sure the above dynamic cast worked.
+  ASSERT_TRUE(camSensor != NULL);
+  EXPECT_TRUE(camSensor->IsActive());
+  camSensor->Update(true);
+
+  int imageCount = 0;
+  this->img = new unsigned char[width*height*3];
+  event::ConnectionPtr c =
+      camSensor->Camera()->ConnectNewImageFrame(
+      std::bind(&Dem_TEST::OnNewCameraFrame, this, &imageCount, this->img,
+      std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+      std::placeholders::_4, std::placeholders::_5));
+
+  // grab some images
+  int sleep = 0;
+  int maxSleep = 500;
+  int total_images = 10;
+  while (imageCount < total_images && sleep++ < maxSleep )
+    common::Time::MSleep(10);
+  EXPECT_GE(imageCount, total_images);
+
+  c.reset();
+
+  unsigned int rSum = 0;
+  unsigned int gSum = 0;
+  unsigned int bSum = 0;
+  for (unsigned int i = 0; i < height*width*3; i+=3)
+  {
+    unsigned int r = img[i];
+    unsigned int g = img[i+1];
+    unsigned int b = img[i+2];
+    rSum += r;
+    gSum += g;
+    bSum += b;
+  }
+
+  // verify that red is the dominant color in the image
+  EXPECT_GT(rSum, gSum);
+  EXPECT_GT(rSum, bSum);
+
+  delete [] this->img;
+}
 #endif
 
 int main(int argc, char **argv)