I am setting up a Gazebo model for use with the navigation stack. I have been reading the Navigation Tuning Guide and am confused about the lidar data in the odom frame. I would think that the tuning guide, when it says:
"The first test checks how reasonable the odometry is for rotation. I open up rviz, set the frame to "odom," display the laser scan the robot provides, set the decay time on that topic high (something like 20 seconds), and perform an in-place rotation. Then, I look at how closely the scans match each other on subsequent rotations. Ideally, the scans will fall right on top of each other, but some rotational drift is expected, so I just make sure that the scans aren't off by more than a degree or two. (Nav Stack Tuning)"
it means that the lidar data is supposed to be in approximately the same place before, during, and after the rotation. I have been reading and it seems that these sweeping swirls that I see are correct? I have been trying to use gmapping in my simulation and whenever I rotate the map gets horridly disfigured - I believe that odometry is to blame.
I have recorded what the lidar data looks like in the odom frame. Is this correct or should it look differently?
I followed this tutorial to build the initial model and simulate it. I created a (visually) crude model with two wheels (left and right) that move and two frictionless casters (front and back) using their general framework. I changed the shape of the robot but just followed their procedure and tried to reproduce it. I have tried to flip the x rotation for the left and right wheels from -pi/2 to pi/2 and that just reversed the direction of motion, which I expected, but does not change the issue of streaky lidar from the odom frame. I am puzzled because the straight odometry data keeps the laser scans in the same position (as one would expect) but when I rotate the robot I get the streaks. I don't really know the mechanism behind calculating the odometry data in Gazebo so I am stuck as to fixing this issue.
The urdf file is shown:
<?xml version="1.0"?>
<robot name="bender" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="pi" value="3.141592653589794" />
<xacro:property name="base_len" value="0.89" />
<xacro:property name="base_wid" value="0.80" />
<xacro:property name="base_height" value="0.20" />
<xacro:property name="caster_length" value="0.10" />
<xacro:property name="caster_radius" value="0.15" />
<xacro:property name="wheel_length" value="0.10" />
<xacro:property name="wheel_radius" value="0.15" />
<xacro:property name="update_rate" value="50"/>
<xacro:property name="hokuyo_size" value="0.05"/>
<xacro:macro name="default_inertial" params="mass">
<inertial>
<mass value="${mass}" />
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
</inertial>
</xacro:macro>
<material name="white">
<color rgba="1 1 1 1.5"/>
</material>
<link name="base_link">
<visual>
<geometry>
<box size="${base_len} ${base_wid} ${base_height}"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<box size="${base_len} ${base_wid} ${base_height}"/>
</geometry>
</collision>
<xacro:default_inertial mass="1"/>
</link>
<xacro:macro name="caster" params="position reflect">
<joint name="${position}_wheel_joint" type="fixed">
<parent link="base_link"/>
<child link="${position}_wheel"/>
<axis xyz="0 0 1"/>
<origin xyz="${reflect*(base_wid/2)} 0 ${-1 * base_height}" rpy="${pi/2} 0 0"/>
</joint>
<link name="${position}_wheel">
<visual>
<geometry>
<sphere radius="${caster_radius}"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<sphere radius="${caster_radius}"/>
</geometry>
</collision>
<xacro:default_inertial mass="0.5"/>
</link>
<!-- This block provides the simulator (Gazebo) with information on a few additional
physical properties. See http://gazebosim.org/tutorials/?tut=ros_urdf for more-->
<gazebo reference="${position}_wheel">
<mu1 value = "0.0"/>
<mu2 value = "0.0"/>
<kp value = "10000000.0"/>
<kd value = "1.0"/>
<material>Gazebo/Grey/</material>
</gazebo>
</xacro:macro>
<xacro:macro name="wheel" params="position reflect">
<joint name="${position}_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="${position}_wheel"/>
<axis xyz="0 0 1"/>
<origin xyz="0 ${reflect*(base_wid/2)} ${-1*base_height}" rpy="${-pi/2} 0 0"/>
</joint>
<link name="${position}_wheel">
<visual>
<geometry>
<cylinder length = "${wheel_length}" radius="${wheel_radius}"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<cylinder length = "${wheel_length}" radius="${wheel_radius}"/>
</geometry>
</collision>
<xacro:default_inertial mass="0.5"/>
</link>
<!-- This block provides the simulator (Gazebo) with information on a few additional
physical properties. See http://gazebosim.org/tutorials/?tut=ros_urdf for more-->
<gazebo reference="${position}_wheel">
<mu1 value = "200.0"/>
<mu2 value = "100.0"/>
<kp value="1000000.0" />
<kd value="1.0" />
<fdir1 value="1 0 0"/>
<material>Gazebo/Grey/</material>
</gazebo>
<!-- This block connects the wheel joint to an actuator (motor), which informs both
simulation and visualization of the robot -->
<transmission name="${position}_wheel_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="${position}_wheel_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="${position}_wheel_joint">
<hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
</joint>
</transmission>
</xacro:macro>
<!-- Creating the actual robot -->
<xacro:caster position="front" reflect="1"/>
<xacro:caster position="back" reflect="-1"/>
<xacro:wheel position="right" reflect="1"/>
<xacro:wheel position="left" reflect="-1"/>
<!-- Gazebo plugin for ROS Control -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
<!-- Hokuyo Laser Senso -->
<link name="hokuyo_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="${hokuyo_size} ${hokuyo_size} ${1.5*hokuyo_size}"/>
</geometry>
<material name="Blue" />
</visual>
</link>
<joint name="hokuyo_joint" type="fixed">
<origin xyz="${base_wid/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />
<parent link="base_link"/>
<child link="hokuyo_link" />
</joint>
<gazebo reference="hokuyo_link">
<material>Gazebo/Blue</material>
<turnGravityOff>false</turnGravityOff>
<sensor type="ray" name="head_hokuyo_sensor">
<pose>${hokuyo_size/2} 0 0 0 0 0</pose>
<visualize>false</visualize>
<update_rate>40</update_rate>
<ray>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-1.570796</min_angle>
<max_angle>1.570796</max_angle>
</horizontal>
</scan>
<range>
<min>0.10</min>
<max>10.0</max>
<resolution>0.001</resolution>
</range>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
</sensor>
</gazebo>
</robot>
I set up my differential drive controller as shown in the tutorial, with my own values, here:
type: "diff_drive_controller/DiffDriveController"
publish_rate: 50
left_wheel: ['left_wheel_joint']
right_wheel: ['right_wheel_joint']
# Odometry covariances for the encoder output of the robot. These values should
# be tuned to your robot's sample odometry data, but these values are a good place
# to start
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Top level frame (link) of the robot description
base_frame_id: base_link
# Velocity and acceleration limits for the robot
# default min = -max
linear:
x:
has_velocity_limits : true
max_velocity : 5.0 # m/s
has_acceleration_limits: true
max_acceleration : 0.6 # m/s^2
angular:
z:
has_velocity_limits : true
max_velocity : 1.0 # rad/s
has_acceleration_limits: true
max_acceleration : 0.5 # rad/s^2
has_jerk_limits : true
max_jerk : 2.5 # rad/s^3
I leave the wheel_separation and wheel_radius out because I wish for them to be generated automatically. I have put in different values, including what I think they are, and it does not change the streaking in lidar data when viewed from the odom frame.
I have also posed the question here.
Edit: I have compiled and ran code from the Book ROS Robotics Projects. Chapter 9 code has a simulated robot. I put fixed frame to odom and when I move forward there is 'streaking' in the x direction but not when I rotate. This makes sense to me but I am wondering why my simulated robot does not behave like this.
