map transform 90 degrees off when using amcl
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 ...
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.