map transform 90 degrees off when using amcl - transforms wrong?

asked 2014-06-06 18:55:33 -0600

curranw gravatar image

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 robotposeekf, robotstatepublisher.

Lastly amcl:

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