point cloud in wrong position relative to robot

asked 2019-07-27 02:18:24 -0500

logan.ydid
21 ●9 ●11 ●15

updated 2019-07-28 19:35:34 -0500

Hello fellow roboticists,

I have been trying to get a kinect sensor working on my urdf model in gazebo. i have successfully attached the sensor thanks to my previous question here. My only issue is that the data is being shown in the wrong place in Rviz. There, the point cloud that should be displayed as being in front of the robot is being displayed above it and rotated about the z axis. How shoud i go about fixing this?

Here is an image of what is happening (left is in Gazebo and the right is in Rviz): image description

and here is my urdf model that i am using:

<?xml version="1.0"?>
<robot name="tortoisebot">

  <link name="base_link">
    <visual>
      <geometry>
        <box size="0.6 0.3 0.3"/>
      </geometry>
      <material name="silver">
        <color rgba="0.75 0.75 0.75 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <box size="0.6 0.3 0.3"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.015" iyy="0.0375" izz="0.0375" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <link name="front_caster">
    <visual>
      <geometry>
        <box size="0.1 0.1 0.3"/>
      </geometry>
      <material name="silver"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.1 0.3"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.00083" iyy="0.00083" izz="0.000167" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <joint name="front_caster_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="front_caster"/>
    <origin rpy="0 0 0" xyz="0.3 0 0"/>
  </joint>

  <link name="front_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <joint name="front_wheel_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <parent link="front_caster"/>
    <child link="front_wheel"/>
    <origin rpy="-1.5708 0 0" xyz="0.05 0 -.15"/>
  </joint>

  <link name="right_wheel">
    <visual>
      <geometry>
        <cylinder length="0.05" radius="0.035"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.035"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="5.1458e-5" iyy="5.1458e-5" izz="6.125e-5" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <joint name="right_wheel_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="right_wheel"/>
    <origin rpy="-1.5708 0 0" xyz="-0.2825 -0.125 -.15"/>
  </joint>

  <link name="left_wheel ...

(more)

edit retag flag offensive close merge delete

add a comment

3 Answers

Sort by » oldest newest most voted

answered 2019-07-29 08:01:45 -0500

MCornelis
331 ●6 ●12 ●11

The static frame of the kinect camera is pretty weird indeed. You basically give the solution to your problem in your question "the point cloud that should be displayed as being in front of the robot is being displayed above it and rotated about the z axis". Simply figure out the correct quaternion transformation you need and apply it to the kinect topic. You can do this by creating another intermediate topic and adding an additional static frame in your launchfile. This problem stems from the fact that the rviz frame does not match the kinects own frame. So for visualizing the kinect model correctly in gazebo and rviz you need 1 transform, but to transform the data you need another.

edit flag offensive delete link

Comments

I had to do this for a project a while back, I currently do not have access to the repository in which I worked at the time. If I can find how exactly I did it back then, I'll come back to you (I'll edit the answer above). In the meantime I hope this answer helps you in the right direction.

MCornelis ( 2019-07-29 08:05:20 -0500 )edit

Im sorry, would you mind elaborating a bit more i found out how to create another static frame in the launch file <node pkg="tf" type="static_transform_publisher" name="laser" args="0 0 0 x y z hokuyo_link kinect_cloud 100"/> but i dont know how to create a new topic such that the data from the old topic gets remapped using the new frame. Thank you!

logan.ydid ( 2019-08-09 00:02:14 -0500 )edit

Were you able to figure out the solution to this problem? My sensor looks the same in rviz.

rodrigo55 ( 2020-03-30 08:01:08 -0500 )edit

add a comment

answered 2020-06-03 11:39:58 -0500

zhubike
11 ●1 ●1

I agree with the top answer. Here raise an example, hope it will be helpful. Pay attention to camera_frame_optical based on kinect model：

 <link name="camera_link">
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <visual>
                <origin xyz="0 0 0" rpy="0 0 ${H_PI/2}"/>
                <geometry>
                    <mesh filename="package://xxxxx/meshes/kinect.dae" />
                </geometry>
            </visual>
            <collision>
                <geometry>
                    <box size="0.07 0.3 0.09"/>
                </geometry>
            </collision>
        </link>

        <joint name="camera_optical_joint" type="fixed">
            <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
            <parent link="camera_link"/>
            <child link="camera_frame_optical"/>
        </joint>
        <link name="camera_frame_optical"/>

       <joint name="camera_frame_joint" type="fixed">
        <origin xyz="${kinect_offset_x} ${kinect_offset_y} ${base_link_height/2+0.02}"
                rpy="0 0 0" />
        <parent link="base_link"/>
        <child link="camera_link"/>
    </joint>


<gazebo reference="camera_link">
    <sensor type="depth" name="camera">
        <always_on>true</always_on>
        <update_rate>20.0</update_rate>
        <camera>
            <horizontal_fov>${60.0*H_PI/180.0}</horizontal_fov>
            <image>
                <format>R8G8B8</format>
                <width>640</width>
                <height>480</height>
            </image>
            <clip>
                <near>0.05</near>
                <far>8.0</far>
            </clip>
        </camera>
        <plugin name="kinect_camera_controller" filename="libgazebo_ros_openni_kinect.so">
            <cameraName>camera</cameraName>
            <alwaysOn>true</alwaysOn>
            <updateRate>10</updateRate>
            <imageTopicName>rgb/image_raw</imageTopicName>
            <depthImageTopicName>depth/image_raw</depthImageTopicName>
            <pointCloudTopicName>depth/points</pointCloudTopicName>
            <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
            <depthImageCameraInfoTopicName>depth/camera_info</depthImageCameraInfoTopicName>
            <frameName>camera_frame_optical</frameName>
            <baseline>0.1</baseline>
            <distortion_k1>0.0</distortion_k1>
            <distortion_k2>0.0</distortion_k2>
            <distortion_k3>0.0</distortion_k3>
            <distortion_t1>0.0</distortion_t1>
            <distortion_t2>0.0</distortion_t2>
            <pointCloudCutoff>0.4</pointCloudCutoff>
        </plugin>
    </sensor>
</gazebo>

edit flag offensive delete link

Comments

It worked for me. Thanks for the Answer. =)

Curiosity_drivenn ( 2022-01-18 09:12:40 -0500 )edit

add a comment

answered 2021-03-12 15:26:24 -0500

Suyashhchougule
11 ●1 ●3 ●5

updated 2021-03-12 16:03:06 -0500

jayess
6155 ●26 ●85 ●92

This worked for me

<node pkg="tf" type="static_transform_publisher" name="base_to_kinect" args=" x y z r p y base_link kinect_camera 100" />

now you can adjust x y z r p y

edit flag offensive delete link

add a comment

point cloud in wrong position relative to robot

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

point cloud in wrong position relative to robot edit

3 Answers

Comments

Comments

Question Tools

Stats

Related questions

point cloud in wrong position relative to robot