0
$\begingroup$

Rosanswers logo

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>

Originally posted by curranw on ROS Answers with karma: 211 on 2014-06-06

Post score: 0


Original comments

Comment by ahendrix on 2014-06-06:
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.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by curranw with karma: 211 on 2014-06-09

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

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