I'm trying to write a plugin to read the depth data of a depth camera.

I can't read those data from the topic because I need to access them while the simulation is paused.

When the simulation is running the camera publishes all the information on the relative topics, both the RGB and depth images. But when the simulation is paused I need a plugin in order to get the information. From the plugin i'm able to retrieve the RGB data using the function ImageData() of the class DepthCameraSensor, but when i try to do the same thing for the depth data using the function DepthData() Gazebo crashes with the message:

Segmentation fault (core dumped)

---

This is the camera urdf:

<?xml version="1.0"?>

<xacro:macro name="kinect_camera_model" params="name parent *origin">
  <joint name="${name}_joint" type="fixed">
    <xacro:insert_block name="origin" />
    <parent link="${parent}"/>
    <child link="${name}_link"/>
  </joint>

  <link name="${name}_link">
    <xacro:inertial_sphere mass="0.01" diameter="0.07" />
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/kinect_camera/kinect_camera_simple.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://hector_sensors_description/meshes/kinect_camera/kinect_camera_simple.stl"/>
      </geometry>
    </collision>
  </link>

  <joint name="${name}_depth_joint" type="fixed">
    <origin xyz="0.0 -0.02 0.0" rpy="0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_depth_frame"/>
  </joint>

  <link name="${name}_depth_frame"/>

  <joint name="${name}_depth_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
    <parent link="${name}_depth_frame" />
    <child link="${name}_depth_optical_frame"/>
  </joint>

  <link name="${name}_depth_optical_frame"/>

  <joint name="${name}_rgb_joint" type="fixed">
    <origin xyz="0.0 -0.0125 0.0" rpy="0 0 0" />
    <parent link="${name}_link" />
    <child link="${name}_rgb_frame"/>
  </joint>

  <link name="${name}_rgb_frame"/>

  <joint name="${name}_rgb_optical_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
    <parent link="${name}_rgb_frame" />
    <child link="${name}_rgb_optical_frame"/>
  </joint>

  <link name="${name}_rgb_optical_frame"/>

</xacro:macro>

<xacro:macro name="kinect_camera" params="name parent *origin">
  <xacro:kinect_camera_model name="${name}" parent="${parent}">
    <xacro:insert_block name="origin" />
  </xacro:kinect_camera_model>

  <gazebo reference="${name}_depth_frame">
    <sensor type="depth" name="${name}">
      <update_rate>50.0</update_rate>
      <camera>
        <horizontal_fov>${60 * M_PI/180.0}</horizontal_fov>
        <image>
          <format>B8G8R8</format>
          <width>100</width> <!-- 640 -->
          <height>30</height> <!-- 480 -->
        </image>
        <depth_camera>
          <output>depths</output>
        </depth_camera>
        <clip>
          <near>0.05</near>
          <far>3</far>
        </clip>
      </camera>
      <plugin name="${name}_camera_controller" filename="libgazebo_ros_openni_kinect.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>0</updateRate>
        <imageTopicName>${name}/rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>${name}/rgb/camera_info</cameraInfoTopicName>
        <depthImageTopicName>${name}/depth/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>${name}/depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>${name}/depth/points</pointCloudTopicName>
        <frameName>${name}_depth_optical_frame</frameName>
      </plugin>
      <plugin name="camera_sensor_reader" filename="libcamera_sensor_reader.so">
        <link>base_link</link>
        <model>quadrotor</model>
      </plugin>
    </sensor>
  </gazebo>
</xacro:macro>

And this is the plugin that I'm using:

#include "CameraSensorReader.hh"

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN(CameraSensorReader);

/////////////////////////////////////////////////
CameraSensorReader::CameraSensorReader() : SensorPlugin()
{
}

/////////////////////////////////////////////////
CameraSensorReader::~CameraSensorReader()
{
}

/////////////////////////////////////////////////
void CameraSensorReader::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
{

  // Get the parent sensor.
  parentSensor = std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor);

  if (!parentSensor)
  {
    gzerr << "CameraPlugin requires a DepthCameraSensor.\n";
    return;
  }

  // Connect to the sensor update event.
  updateConnection = parentSensor->ConnectUpdated(std::bind(&CameraSensorReader::OnUpdate, this));

  // Make sure the parent sensor is active.
  parentSensor->SetActive(true);

  if (_sdf->HasElement("link"))
  {
    link_name = _sdf->GetElement("link")->Get<std::string>();
    service_name = _sdf->GetElement("model")->Get<std::string>() +"/"+ link_name +"/getDepthImage";
  }

  if (!ros::isInitialized())
    {
      int argc = 0;
      char **argv = NULL;
      ros::init(argc, argv, "gazebo", ros::init_options::NoSigintHandler);
    }

  rosNode.reset(new ros::NodeHandle(""));

  camera_server = rosNode->advertiseService(service_name , &CameraSensorReader::GetDepthImage, this);
}

/////////////////////////////////////////////////
void CameraSensorReader::OnUpdate()
{

}


bool CameraSensorReader::GetDepthImage(drone_msgs::GetDepthImage::Request& req, drone_msgs::GetDepthImage::Response& resp)
{
  const float *depthImagePointer;
  depthImagePointer = parentSensor->DepthData();

  int width = parentSensor->ImageWidth();
  int height = parentSensor->ImageHeight();

  std::vector<float> depthImage;

  for(int i = 0; i < width*height; i++)
  {
      depthImage.push_back(depthImagePointer[i]);
  }

  resp.depthImage = depthImage;
  resp.width = width;
  resp.height = height;

  return true;
}

The problem arises when I try to access depthImagePointer[i] in the for loop.