Revision history [back]

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" pkg="amcl" type="amcl" output="screen">
    <remap from="scan" to="/wheelchair_lasers/right" />
    <param name="odom_frame_id" value="odom_combined"/>
    <param name="base_frame_id" value="base_link"/>
</node>

Tf Frames: I've tried changing the left_scanner to be of the same tf as the rest of the robot, same results. I cannot upload files yet, not enough karma.

tf_vis

tf_tree

One possible hack fix that may shed light on the problem.

rosrun tf static_transform_publisher 0 0 0 0 1.5 /map /odom 10

Fixes the problem, so it's the transformation between odom and map. My odometry is simply created by gazebo:

<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50</updateRate>
    <leftJoint>left_main_wheel_joint</leftJoint>
    <rightJoint>right_main_wheel_joint</rightJoint>
    <wheelSeparation>${chair_width}</wheelSeparation>
    <wheelDiameter>${main_wheel_radius*2}</wheelDiameter>
    <torque>20</torque>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_footprint</robotBaseFrame>
  </plugin>
</gazebo>