0
$\begingroup$

Rosanswers logo

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?


Edit 1: 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.


Edit 2: 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.


Originally posted by cmfuhrman on ROS Answers with karma: 200 on 2018-02-26

Post score: 0


Original comments

Comment by lucasw on 2018-02-26:
It shouldn't swirl like that. The gazebo robot appears to be rotating the opposite direction as in rviz - or is the rviz display upside down looking through the floor?

Comment by cmfuhrman on 2018-02-26:
Should the robot spawn in the same direction in both? I didn't set any parameters it just was that way. When I went through URDF tutorials the R2D2 we made appeared that way. Plus, it drives forward the way I expect and the lidar scanning with large decay stays in a constant position.

Comment by lucasw on 2018-02-26:
Maybe there is a way to flip the direction of the odometry rotation (the crudest way would be to subscribe to it, flip the sign, then republish under a new name and have rviz use that.)

Comment by lucasw on 2018-02-26:
Do you have a custom urdf you can post? Update the question with it if so. What is the link to the tutorial you are following, and are you following it exactly or what changes have you made?

Comment by cmfuhrman on 2018-02-27:
I have been following another example as outlined in Edit 2 in above post. The 'streaking' behavior occurs in the x direction rather than when rotating. What could make this occur?

Comment by lucasw on 2018-02-28:
If you are able to individually turn wheels it would be good to verify that the left wheel in gazebo is the left wheel in rviz and is turning in a consistent direction and the same for the right wheel.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

I checked your repository, noticed that when sending the command to turn right it does so in rviz, but turns left in gazebo.

That is because the wheels are turning in the wrong direction, both wireframe mode in gazebo and tf in rviz do show that. To fix that you need to rotate both wheels by 180 degree.

Removing the minus sign of rpy in your model.urdf.xacro, line 102 should do that:

<origin xyz="0 ${reflect*(base_wid/2)} ${-1*base_height}" rpy="${-pi/2} 0 0"/>

Originally posted by Humpelstilzchen with karma: 1504 on 2018-02-28

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by cmfuhrman on 2018-02-28:
You are definitely right! I also think that the left and right wheels were on the wrong side of the bot. I think this is going in the right direction, no? Thank you!

Comment by cmfuhrman on 2018-03-01:
Update: I incorporated your feedback, and changed the configuration of the wheels to be on the correct sides of the robot (DUH!!). I also removed the base_footprint for now and I was able to map the simulated environment with gmapping - showing that odometry is working properly.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.