Otomap_server octomap mapping problem
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:
I am trying to simulate this in gazebo:
But it seems the map is not correct after I manually moving the drone (Map looks good if I don't move the drone):
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< ...
why do you attach the camera_link to world instead of base_link
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?
Oh, I realized do I need also to broadcast a transform from camera_link to base_link using a ros node?
in gazebo ,
world
is static coordinate system, all other existing frames should be defined w.r.t thisworld
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
) toworld
, 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;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 :)
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?
yes, depending on the position of camera on robot, a joint that connect these two links should exist, for instance
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