diff_drive_controller odometry issues
Hello everyone!
I have following issue with diffdrivecontroller I really don't know how to solve. I have an sdf defined robot, which uses odometry published by the controller defined as follows:
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<robotNamespace />
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>right_wheel_joint</leftJoint>
<rightJoint>left_wheel_joint</rightJoint>
<wheelSeparation>0.5380</wheelSeparation>
<wheelDiameter>0.2410</wheelDiameter>
<torque>20</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>chassis</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishWheelJointState>false</publishWheelJointState>
<rosDebugLevel>na</rosDebugLevel>
<wheelAcceleration>0</wheelAcceleration>
<wheelTorque>5</wheelTorque>
<odometrySource>world</odometrySource>
<publishTf>1</publishTf>
</plugin>
The problem is that when robot starts navigating from the origin of the world, everything is fine, but when I slightly shift it away, the robot's odometry jumps and shows incorrect values. Video of shifted and properly placed robots is attached below: https://www.youtube.com/watch?v=vvPnN-XJeq8
I really need the robot to be placed somewhere not in the origin. I would really appreciate any help and suggestions.
Here's the full .sdf configuration of my robot, if necessary:
<?xml version="1.0" encoding="UTF-8"?>
<link name="chassis">
<pose>3 0 .05 0 0 0</pose>
<inertial>
<inertia>
<ixx>0</ixx>
<iyy>0</iyy>
<izz>0</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
<mass>4.5</mass>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>.9 .675 .15</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>.9 .675 .15</size>
</box>
</geometry>
<material>
<ambient>1 0.5 0 1</ambient>
<diffuse>1 0.5 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>
<collision name="caster_collision_front">
<pose>0.40 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="caster_visual_front">
<pose>0.40 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
</visual>
<collision name="caster_collision_back">
<pose>-0.40 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
<surface>
<friction>
<ode>
<mu>0.0</mu>
<mu2>0.0</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
</ode>
</friction>
</surface>
</collision>
<visual name="caster_visual_back">
<pose>-0.40 0 -0.05 0 0 0</pose>
<geometry>
<sphere>
<radius>.05</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="left_wheel">
<pose>3 0.30 0.05 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
<link name="right_wheel">
<pose>3 -0.30 0.05 0 1.5707 1.5707</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.1</radius>
<length>.05</length>
</cylinder>
</geometry>
</visual>
</link>
<!-- Hokuyo laser -->
<link name="laser">
<pose>3.139000 0.000000 0.15 0.000000 0.000000 0.000000</pose>
<gravity>0</gravity>
<inertial>
<mass>0.100000</mass>
<inertia>
<ixx>1.000000</ixx>
<ixy>0.000000</ixy>
<ixz>0.000000</ixz>
<iyy>1.000000</iyy>
<iyz>0.000000</iyz>
<izz>1.000000</izz>
</inertia>
</inertial>
<visual name="visual">
<geometry>
<mesh>
<uri>model://hokuyo/meshes/hokuyo.dae</uri>
</mesh>
</geometry>
</visual>
<collision name="collision-base">
<pose>0.000000 0.000000 -0.014500 0.000000 0.000000 0.000000</pose>
<geometry>
<box>
<size>0.050000 0.050000 0.041000</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
</collision>
<collision name="collision-top">
<pose>0.000000 0.000000 0.020500 0.000000 0.000000 0.000000</pose>
<geometry>
<cylinder>
<radius>0.021000</radius>
<length>0.029000</length>
</cylinder>
</geometry>
<max_contacts>10</max_contacts>
</collision>
<sensor name="laser" type="ray">
<ray>
<scan>
<horizontal>
<samples>180</samples>
<resolution>1.000000</resolution>
<min_angle>-1.570000</min_angle>
<max_angle>1.570000</max_angle>
</horizontal>
</scan>
<range>
<min>0.080000</min>
<max>10.000000</max>
<resolution>0.010000</resolution>
</range>
</ray>
<update_rate>20.000000</update_rate>
<always_on>1</always_on>
<visualize>1</visualize>
<!-- ROS Laser Plugin -->
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">
<topicName>laser/scan</topicName>
<frameName>hokuyo_link</frameName>
<robotNamespace></robotNamespace>
</plugin>
</sensor>
<velocity_decay>
<linear>0.000000</linear>
<angular>0.000000</angular>
</velocity_decay>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
</link>
<!-- Lift platform -->
<link name="top_lift">
<pose>3 0 0.2 0 0 0</pose>
<collision name="collision">
<geometry>
<cylinder>
<radius>.3375</radius>
<length>.03</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>Inf</mu>
<mu2>Inf</mu2>
<slip1>0.0</slip1>
<slip2>0.0</slip2>
<!--<implicit_spring_damper>1</implicit_spring_damper>
<maxVel>0.0</maxVel>
<minDepth>0.001</minDepth>-->
</ode>
</friction>
</surface>
</collision>
<visual name="visual">
<geometry>
<cylinder>
<radius>.3375</radius>
<length>.03</length>
</cylinder>
</geometry>
</visual>
</link>
<!--
<joint type="screw" name="top_joint_lift_hinge">
<pose>0 0 0.2 0 0 0</pose>
<child>top_lift</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 -1</xyz>
<limit>
<upper>1</upper>
<lower>0</lower>
</limit>
</axis>
<thread_pitch>31.416</thread_pitch>
</joint>
-->
<!-- Prismatic joint -->
<joint type="prismatic" name="top_joint_lift_hinge">
<pose>0 0 0.2 0 0 0</pose>
<child>top_lift</child>
<parent>chassis</parent>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0.05</upper>
<lower>0</lower>
</limit>
</axis>
</joint>
<!-- Screw rotate ends here -->
<joint type="revolute" name="left_wheel_joint">
<pose>0 0 -0.03 0 0 0</pose>
<child>left_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint type="revolute" name="right_wheel_joint">
<pose>0 0 0.03 0 0 0</pose>
<child>right_wheel</child>
<parent>chassis</parent>
<axis>
<xyz>0 1 0</xyz>
</axis>
</joint>
<joint name="joint_laser" type="fixed">
<parent>chassis</parent>
<child>laser</child>
<axis>
<xyz>0.000000 0.000000 0.000000</xyz>
<limit>
<lower>0.000000</lower>
<upper>0.000000</upper>
</limit>
</axis>
</joint>
<!-- Attach the screw plugin to this model-->
<plugin name="prismatic_joint_control" filename="libgazebo_ros_screw_plugin.so">
<jointForce>0</jointForce>
</plugin>
<!-- Attach the ROS Differential Drive Plugin to this model> -->
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<robotNamespace />
<alwaysOn>true</alwaysOn>
<updateRate>10</updateRate>
<legacyMode>true</legacyMode>
<leftJoint>right_wheel_joint</leftJoint>
<rightJoint>left_wheel_joint</rightJoint>
<wheelSeparation>0.5380</wheelSeparation>
<wheelDiameter>0.2410</wheelDiameter>
<torque>20</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>chassis</robotBaseFrame>
<publishWheelTF>false</publishWheelTF>
<publishWheelJointState>false</publishWheelJointState>
<rosDebugLevel>na</rosDebugLevel>
<wheelAcceleration>0</wheelAcceleration>
<wheelTorque>5</wheelTorque>
<odometrySource>world</odometrySource>
<publishTf>1</publishTf>
</plugin>
Asked by kurshakuz on 2019-08-13 11:11:31 UTC
Answers
You are using a static transform between /map and /amazon_warehouse_robot/odom and "world" as the odometrySource for the libgazebo_ros_diff_drive plugin.
Using "world" as the source for the odometry of your simulated robot will give you the perfect location of the robot in the simulated world. So when you set the static transform between /map and /amazon_warehouse_robot/odom to be 0 0 0 0 0 0 everything works. But if you shift the static transform, then nothing makes sense any more.
If you want to shit the starting pose of the robot you should do it by moving the robot inside the gazebo world, not by modifying the static transform.
Any way, in the Real World™ this will not work because the odometry is far (faaaaar, faaaaaaaaaaar, very faaaaaaaaaaaaaaaaar) from perfect. You should take a look at amcl to handle the transform between /map and /amazon_warehouse_robot/odom instead of using a static transform.
Asked by Martin Peris on 2019-08-14 01:59:56 UTC
Comments
Hi, thanks for these comments. I have actually tried spawning shifted robot with static transform between /amp and /amazon_warehouse_robot/odom at 0 0 0 0 0 0, but the problem was that the rViz was showing location of the robot at the origin while it was not the case. Additionally, I have previously used AMCL, and performance was almost the same. The configurations for it were as follows:
Asked by kurshakuz on 2019-08-14 02:25:11 UTC
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<param name="odom_model_type" value="diff"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="10.0"/>
<param name="odom_frame_id" value="$(arg odom_frame_id)"/>
<param name="base_frame_id" value="$(arg base_frame_id)"/>
<remap from="scan" to="$(arg scan_topic)"/>
<param name="global_frame_id" value="$(arg global_frame_id)"/>
</node>
Asked by kurshakuz on 2019-08-14 02:25:37 UTC
Comments