ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
2

libgazebo_ros_openni_kinect.so depth pointcloud wrong tf

asked 2015-09-29 04:38:54 -0500

Jan E. gravatar image

Hello, I am new to ROS. I am trying to simulate a robot in gazebo witch works fine for now, but I have a problem with simulating a kinect camera. The simulated data seems to be right, but when I try to display the results in rviz the transform is not correct. Can anyone give me a hint where my coding went wrong?

Gazebo: 6.1.0 Ros: Indigo Rviz: 1.11.8

image description

model.sdf


<link name="CameraLeft_Link">
  <pose>0.29 0.0 1.068 0.05 0.535 0.13</pose>
      <inertial>
        <mass>0.1</mass>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>0.1 0.1 0.1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <pose>0 0 0 1.57 0 1.57</pose> 
        <geometry>
          <mesh>
              <uri>model://b45/meshes/asus.dae</uri>
            </mesh>
        </geometry>
      </visual>



<sensor type="depth" name="openni_camera_camera">      
        <always_on>1</always_on>
        <visualize>true</visualize>             
        <camera>
            <horizontal_fov>1.3962634</horizontal_fov>  
            <image>
                <width>640</width>
                <height>480</height>
                <format>R8G8B8</format>
            </image>

            <clip>
                <near>0.1</near>
                <far>100</far>
            </clip>
        </camera>
        <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
            <alwayson>true</alwayson>
                <updaterate>30.0</updaterate>
                <cameraname>cameraLeft</cameraname>
                <framename>/CameraLeft_Link</framename>                   
            <imagetopicname>CameraLeft/rgb/image_raw</imagetopicname>
            <depthimagetopicname>CameraLeft/depth/image_raw</depthimagetopicname>
            <pointcloudtopicname>CameraLeft/depth/points</pointcloudtopicname>
            <camerainfotopicname>CameraLeft/rgb/camera_info</camerainfotopicname>              
            <depthimagecamerainfotopicname>CameraLeft/depth/camera_info</depthimagecamerainfotopicname>            
            <pointcloudcutoff>0.8</pointcloudcutoff>
            <pointcloudcutoffmax>3.5</pointcloudcutoffmax>          
            </plugin>
    </sensor>



    </link>

robot.urdf



<robot name="autreim">

<link name="base_link">
</link>



  <joint name="laser" type="fixed">
    
    <origin xyz="0.46 0 0.62" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="base_laser_link"/>
  </joint>
  <link name="base_laser_link">
  </link>

<joint name="cameraLeft" type="fixed">
    <origin xyz="0.29 0.0 1.068" rpy="0.05 0.535 0.13"/>
    <parent link="base_link"/>
    <child link="CameraLeft_Link"/>
  </joint>
  <link name="CameraLeft_Link">
  </link>

<joint name="cameraRight" type="fixed">
    <origin xyz="0.305 -0.22 0.975" rpy="1.52 0.4 -1.08"/>
    <parent link="base_link"/>
    <child link="CameraRight_Link"/>
  </joint>
  <link name="CameraRight_Link">
  </link>

  <joint name="scheuerteller" type="fixed">
    <origin xyz="0.33 -0.06 0" rpy="0 0 0"/>
    <child link="scheuerteller_link"/>
    <parent link="base_link"/>
  </joint>
  <link name="scheuerteller_link"/>

  <joint name="footprint" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>
  <link name="base_footprint"/>

  <joint name="ximu" type="fixed">
    
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="imu"/>
  </joint>
  <link name="imu"/> 


<joint name="wheelodom" type="fixed">
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <child link="wheelodom"/>
    <parent link="base_link"/>
  </joint>
  <link name="wheelodom"/>

</robot>
edit retag flag offensive close merge delete

Comments

Whether you tried by changing the corresponding joint orientation?

SVS gravatar image SVS  ( 2015-09-29 06:56:39 -0500 )edit

sdf


<pose>0.29 0.0 1.068 0.05 0.535 0.13</pose>

urdf


<origin xyz="0.29 0.0 1.068" rpy="0.05 0.535 0.13"/>

When I change it


<origin xyz="0.29 0.0 1.068" rpy="1.035 3.1 1.675"/>

the pointcloud fits the laserscan. But why?

Jan E. gravatar image Jan E.  ( 2015-09-30 02:48:56 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
5

answered 2015-10-07 01:12:46 -0500

Jan E. gravatar image

By adding a link for the depthframe I got rid of the wrong transform.




  <joint name="cameraLeft_joint" type="fixed">
    <origin xyz="0 0.11 0.02" rpy="0 0.39 1"/>
    <parent link="cameraBase_link"/>
    <child link="cameraLeft_link"/>
  </joint>

  <link name="cameraLeft_link">
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.05"/>
      </geometry>
    </collision>

    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
    <box size="0.05 0.05 0.05"/>
      </geometry>
      <material name="orange"/>
    </visual>

    <inertial> 
      <origin xyz="0 0 0" rpy="0 0 0"/> 
      <mass value="0.1"/> 
      <box_inertia m="0.1" x="0.05" y="0.05" z="0.05"/>
    </inertial>
  </link>

  
  <joint name="cameraLeft_depth_joint" type="fixed">
    <origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2} "/>
    <parent link="cameraLeft_link"/>
    <child link="cameraLeft_depth_link"/>
  </joint>

  <link name="cameraLeft_depth_link"></link>


  <gazebo reference="cameraLeft_link">
    <sensor type="depth" name="openni_camera_camera">      
        <always_on>1</always_on>
        <visualize>true</visualize>             
        <camera>
            <horizontal_fov>1.3962634</horizontal_fov>  
            <image>
                <width>640</width>
                <height>480</height>
                <format>R8G8B8</format>
            </image>

            <clip>
                <near>0.1</near>
                <far>100</far>
            </clip>
        </camera>
        <plugin name="cameraLeft_controller" filename="libgazebo_ros_openni_kinect.so">
            <alwayson>true</alwayson>
                <updaterate>30.0</updaterate>
                <cameraname>cameraLeft</cameraname>
                <framename>cameraLeft_depth_link</framename>                  
            <imagetopicname>CameraLeft/rgb/image_raw</imagetopicname>
            <depthimagetopicname>CameraLeft/depth/image_raw</depthimagetopicname>
            <pointcloudtopicname>CameraLeft/depth/points</pointcloudtopicname>
            <camerainfotopicname>CameraLeft/rgb/camera_info</camerainfotopicname>              
            <depthimagecamerainfotopicname>CameraLeft/depth/camera_info</depthimagecamerainfotopicname>            
            <pointcloudcutoff>0.8</pointcloudcutoff>
            <pointcloudcutoffmax>3.5</pointcloudcutoffmax>          
            </plugin>
    </sensor>
  </gazebo>
edit flag offensive delete link more

Comments

1

Thanks, @Jan E. It helps me too.


<joint name="cameraLeft_depth_joint" type="fixed">
  <origin xyz="0 0 0" rpy="${-PI/2} 0 ${-PI/2} "/>
  <parent link="cameraLeft_link"/>
  <child link="cameraLeft_depth_link"/>
</joint>
<link name="cameraLeft_depth_link"/>

vovka gravatar image vovka  ( 2016-02-21 08:57:11 -0500 )edit

clever solution by adding empty link. The reason for this issue is that the gazebo plugin is not publishing /tf transforms like real openni driver do, "link" refers to physical setup, where frame is for the images. In openni_wrapper, 2 frame transforms will be published with < camera_link > as parent, but not in gazebo.

Fookii gravatar image Fookii  ( 2020-04-13 14:20:28 -0500 )edit

Thank you so much this worked for me.

Suyashhchougule gravatar image Suyashhchougule  ( 2021-03-13 03:27:24 -0500 )edit

Thanks for saving my day

prabin_rath gravatar image prabin_rath  ( 2022-05-21 06:46:00 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2015-09-29 04:26:55 -0500

Seen: 2,875 times

Last updated: Oct 07 '15