Ask Your Question
0

map transform 90 degrees off when using amcl

asked 2014-06-06 14:40:39 -0500

curranw gravatar image

updated 2014-06-06 16:36:42 -0500

Hello,

I've been building a custom robot in gazebo. This robot has two laser scanners, one on each side facing outward (not forward). When running amcl, the laser scans are 90 degrees off of where they should be. If I set the 2d nav position to the correct position, the laser scans are 90 degrees off. If I set it 90 degrees offset, the laserscans are correct. The odd thing is, no part of my urdf is rotated 90 degrees (except the wheels). Here is my process:

I start up gazebo and load up my model. Here are the urdf laser scan links and the laser scan plugin. There are no rpy rotations at any links above the laser scans.

  <link name="right_scanner">
    <visual>
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.05 0.05 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-5" />
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
    </inertial>
  </link>

  <joint name="right_scanner_joint" type="fixed">
    <axis xyz="0 0 0" />
    <origin xyz="${0.05/2} -${chair_length/2 - 0.05/2 + chair_length/8} ${chair_height/16 + 0.05}" rpy="0 0 0"/>
    <parent link="right_bar"/>
    <child link="right_scanner"/>
  </joint>

  <link name="left_scanner">
    <visual>
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.05 0.05 0.1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.05 0.05 0.1"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-5" />
      <inertia ixx="1e-3" ixy="0" ixz="0" iyy="1e-3" iyz="0" izz="1e-3" />
    </inertial>
  </link>

  <joint name="left_scanner_joint" type="fixed">
    <axis xyz="0 0 0" />
    <origin xyz="-${0.05/2} -${chair_length/2 - 0.05/2 + chair_length/8} ${chair_height/16 + 0.05}" rpy="0 0 -3.14"/>
    <parent link="left_bar"/>
    <child link="left_scanner"/>
  </joint>

    <gazebo reference="right_scanner">
    <sensor type="gpu_ray" name="head_hokuyo_sensor">
      <pose>0 0 0 0 0 0</pose>
      <visualize>true</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-1.57079633</min_angle>
            <max_angle>1.57079633</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
        <topicName>/wheelchair_lasers/right</topicName>
        <frameName>right_scanner</frameName>
      </plugin>
    </sensor>
  </gazebo>

   <gazebo reference="left_scanner">
    <sensor type="gpu_ray" name="head_hokuyo_sensor2">
      <pose>0 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.57079633</min_angle>
            <max_angle>1.57079633</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin name="gazebo_ros_head_hokuyo_controller2" filename="libgazebo_ros_gpu_laser.so">
        <topicName>/wheelchair_lasers/left</topicName>
        <frameName>left_scanner</frameName>
      </plugin>
    </sensor>
  </gazebo>

I then run robot_pose_ekf, robot_state_publisher.

Lastly amcl:

<node name="map_server" pkg="map_server" type="map_server" args="$(find wheelchair_mapping)/map.yaml" />
<node name="amcl ...
(more)
edit retag flag offensive close merge delete

Comments

If you visualize the TF frames in rviz, do the laser scans line up with their origin frames properly? Which frame are you sending your position estimate in? A screenshot might be helpful.

ahendrix gravatar imageahendrix ( 2014-06-06 14:50:08 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2014-06-09 16:59:09 -0500

curranw gravatar image

updated 2014-06-09 16:59:37 -0500

Found the solution.

The robot was actually built entirely 90 degrees off center. Swapping all of my x,y values in origin xyz and rpy, as well as the x,y axis values fixed everything.

Thanks for the help.

edit flag offensive delete link more
0

answered 2014-06-06 15:41:35 -0500

ahendrix gravatar image

Your right scanner is pointing forward and your left scanner is pointing backwards; is that what you intended? Note that in ROS, X is forward; this corresponds to the green axes in rviz.

In your URDF, you've specified the yaw of the left scanner as -3.14 (180 degrees) and the yaw of the right scanner as 0. This doesn't match your description of your robot; you probably want the yaw for the left scanner to be 1.57 and the yaw for your right scanner to be -1.57.

edit flag offensive delete link more

Comments

Those numbers are an artifact of me trying to figure out what was going on. I changed the yaw to see if the map tf frame would rotate with the scanner. Your yaw numbers along with 0 to 3.14 degree min/max angle results in the same laser scan as my numbers with -1.57 to 1.57. I just double checked and the same 90 degree error exists. If I change the fixed frame to the right_scanner, and visualize the right scans, they are in the right spot. It seems like the tf transform for the map is the problem. So what creates the map tf transform? Map_server publishes it. It's likely computed in gmapping. So the problem must be with that somehow. This is where I am stumped.

curranw gravatar imagecurranw ( 2014-06-06 15:59:49 -0500 )edit

I'm not sure I understand why you're adjusting the laser min and max angle.

ahendrix gravatar imageahendrix ( 2014-06-06 16:36:07 -0500 )edit

If you set the fixed frame to 'odom' or 'base_link' in rviz, you should be able to visualize your laser scans with respect to the rest of your TF tree, without running AMCL at all.

ahendrix gravatar imageahendrix ( 2014-06-06 16:37:22 -0500 )edit

I was adjusting the yaw of the lasers, which made me need to adjust the min and max angle. If I set my fixed frame to odom or base_link in rviz, the laser scans are accurate. The transform between map and odom is the problem. This didn't solve my problem, but the answer is related: http://answers.ros.org/question/104111/how-to-publish-transform-from-odom-to-base_link/ I also updated the question with a hack-fix that might shed light on the problem.

curranw gravatar imagecurranw ( 2014-06-06 17:49:11 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2014-06-06 14:40:39 -0500

Seen: 806 times

Last updated: Jun 09 '14