Problem with the function DepthData() of the class DepthCameraSensor

asked 2020-08-08 10:51:30 -0600

Gianluca gravatar image

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:

<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <xacro:include filename="$(find hector_xacro_tools)/urdf/inertia_tensors.urdf.xacro"/> <xacro:property name="M_PI" value="3.1415926535897931"/>

<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>

</robot>

And this is the plugin that I'm using:

#include "CameraSensorReader.hh"

using namespace gazebo;
GZ_REGISTER_SENSOR_PLUGIN ...
(more)
edit retag flag offensive close merge delete