Otomap_server octomap mapping problem

asked 2020-02-09 22:15:37 -0500

zhefan gravatar image

updated 2020-02-09 23:40:21 -0500

I am using octomap_mapping to generate map in Rviz.

<launch> <node pkg="octomap_server" type="octomap_server_node" name="octomap_server">

    <!-- fixed map frame (set to 'map' if SLAM or localization running!) -->
    <param name="frame_id" type="string" value="world" />

    <!-- maximum range to integrate (speedup!) -->
    <param name="sensor_model/max_range" value="5.0" />

    <!-- data source to integrate (PointCloud2) -->
    <remap from="cloud_in" to="/camera/depth/points" />

TF looks like this:

image description

I am trying to simulate this in gazebo:

image description

But it seems the map is not correct after I manually moving the drone (Map looks good if I don't move the drone):

image description

I think octomap_server doesn't require you to align pointclouds, so what is the reason for my problem? Thanks in advance!!!

Drone Model:

    <model name='sjtu_drone'>
  <plugin name='simple_drone' filename='libplugin_drone.so'>
    <bodyName>base_link</bodyName>
    <imuTopic>/drone/imu</imuTopic>
    <rollpitchProportionalGain>10.0</rollpitchProportionalGain>
    <rollpitchDifferentialGain>5.0</rollpitchDifferentialGain>
    <rollpitchLimit>0.5</rollpitchLimit>
    <yawProportionalGain>2.0</yawProportionalGain>
    <yawDifferentialGain>1.0</yawDifferentialGain>
    <yawLimit>1.5</yawLimit>
    <velocityXYProportionalGain>5.0</velocityXYProportionalGain>
    <velocityXYDifferentialGain>2.3</velocityXYDifferentialGain>
    <velocityXYLimit>2</velocityXYLimit>
    <velocityZProportionalGain>5.0</velocityZProportionalGain>
    <velocityZDifferentialGain>1.0</velocityZDifferentialGain>
    <velocityZLimit>-1</velocityZLimit>
    <positionXYProportionalGain>1.1</positionXYProportionalGain>
    <positionXYDifferentialGain>0.0</positionXYDifferentialGain>
    <positionXYIntegralGain>0.0</positionXYIntegralGain>
    <positionXYLimit>5</positionXYLimit>
    <positionZProportionalGain>1.0</positionZProportionalGain>
    <positionZDifferentialGain>0.2</positionZDifferentialGain>
    <positionZLimit>-1</positionZLimit>
    <maxForce>30</maxForce>
    <motionSmallNoise>0.05</motionSmallNoise>
    <motionDriftNoise>0.03</motionDriftNoise>
    <motionDriftNoiseTime>5.0</motionDriftNoiseTime>
  </plugin>
  <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50.0</updateRate>
    <bodyName>base_link</bodyName>
    <topicName>odom</topicName>
    <gaussianNoise>0.01</gaussianNoise>
    <frameName>world</frameName>
    <xyzOffsets>0 0 0</xyzOffsets>
    <rpyOffsets>0 0 0</rpyOffsets>
  </plugin>
  <pose frame=''>0 1 1 0 -0 1.6</pose>
  <link name='base_link'>
    <inertial>
      <mass>1.477</mass>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <inertia>
        <ixx>0.1152</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>0.1152</iyy>
        <iyz>0</iyz>
        <izz>0.218</izz>
      </inertia>
    </inertial>
    <visual name='visual'>
      <geometry>
        <mesh>
          <uri>file://models/sjtu_drone/quadrotor_4.dae</uri>
        </mesh>
      </geometry>
    </visual>
    <collision name='collision'>
      <geometry>
        <mesh>
          <uri>file://models/sjtu_drone/quadrotor_4.dae</uri>
        </mesh>
      </geometry>
      <max_contacts>10</max_contacts>
      <surface>
        <contact>
          <ode/>
        </contact>
        <bounce/>
        <friction>
          <torsional>
            <ode/>
          </torsional>
          <ode/>
        </friction>
      </surface>
    </collision>
    <gravity>1</gravity>
    <sensor name='sensor_imu' type='imu'>
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose frame=''>0 0 0 0 -0 0</pose>
      <plugin name='ros_imu' filename='libplugin_ros_imu.so'/>
      <imu>
        <angular_velocity>
          <x>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </x>
          <y>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </y>
          <z>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </z>
        </angular_velocity>
        <linear_acceleration>
          <x>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </x>
          <y>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </y>
          <z>
            <noise type='gaussian'>
              <mean>0</mean>
              <stddev>0</stddev>
            </noise>
          </z>
        </linear_acceleration>
      </imu>
    </sensor>
    <sensor name='camera2' type='depth'>
      <camera name='__default__'>
        <horizontal_fov>1.8</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0< ...
(more)
edit retag flag offensive close merge delete

Comments

why do you attach the camera_link to world instead of base_link

Fetullah Atas gravatar image Fetullah Atas  ( 2020-02-09 23:18:28 -0500 )edit

Hi atas, good to see your reply again! I tried to visualize pointcloud in rviz. Only camera link can see the pointcloud, if I use base link, there is no pointcloud. So, what is wrong with using camera link? Again, I think it is because I might still got some problem with my drone model with depth camera this time. I will update the model in question. Can you help me figure out why my base_link does not work?

zhefan gravatar image zhefan  ( 2020-02-09 23:39:38 -0500 )edit

Oh, I realized do I need also to broadcast a transform from camera_link to base_link using a ros node?

zhefan gravatar image zhefan  ( 2020-02-09 23:42:32 -0500 )edit

in gazebo , world is static coordinate system, all other existing frames should be defined w.r.t this world frame. When your robot is moving around in this world, you usually have /odom frame which provides continuous transform from your robot center(/base_link) to world, Assuming your robot is moving and exploring around, it will make more sense if camera is attached to robot(could be base_link). If you attach the camera to base_link you do not need to broadcast additional transform, and probably the reason you do not see point cloud is, you have not defined a connection between base_link and camera_link, so in RVIZ you would be seeing something like;

No transform from [blabla_link] to [/some_link]
Fetullah Atas gravatar image Fetullah Atas  ( 2020-02-09 23:53:11 -0500 )edit

in simulation you may not need to have /odom frame, because youy already have instant access to robot's pose, so do not be confused by that :)

Fetullah Atas gravatar image Fetullah Atas  ( 2020-02-09 23:55:03 -0500 )edit

Thanks! That makes sense! I could understand why base_link does not have pointcloud, but why using camera_link won't work (I mean for octomap_mapping)? The camera is in the same position as robot, the coordinate frame should be almost the same, isn't it? You mentioned I haven't defined a connection between base_link and camera_link, does that mean I need some <joint> in my model to connect them?

zhefan gravatar image zhefan  ( 2020-02-10 00:17:45 -0500 )edit

yes, depending on the position of camera on robot, a joint that connect these two links should exist, for instance

<joint name="camera_joint" type="fixed">
    <parent link="base_link" />
    <child link="camera_link" />
    <origin rpy="0 1.57 0" xyz="0.2 -0.2 0.2" />
    <axis xyz="0 0 0" />
</joint>
Fetullah Atas gravatar image Fetullah Atas  ( 2020-02-10 00:24:01 -0500 )edit

Have look at this URDF file of simulated car in gazebo ,with various sensor attcahed on. try to duplicate it according to your sensors and links

Fetullah Atas gravatar image Fetullah Atas  ( 2020-02-10 00:27:00 -0500 )edit