Robotics StackExchange | Archived questions

GAZEBO SPAWNING UNWANTED ODOM TO BASE LINK TRANSFORM. FIX NEEDED.

Hi all, I built my custom 4wheeler and after spawning it in gazebo, everything worked out just fine. I then decided to bring it alive by including a diffdrive controller. At this point, it all goes wrong. When I spawn the robot, in Gazebo it looks okay. But in RVIZ, there's a complaint about missing transforms between the basefootprint and the rest of the robot. As you can see from the TF diagram attached, the basefootprint link has been displaced by a new connection between the odom frame and the baselink frame. See my Robot URDF bellow

image description

<?xml version = "1.0"?>

/xacro:macro


/xacro:macro


/xacro:macro


<!--limit effort="1000.0" lower="${-pi/8}" upper="${pi/8}" velocity="0.5"/> For setting joint mobility around the Y axis --> 0.00001 Gazebo/Grey /xacro:macro

<!--Reflect1 represents left-right reflect, reflect2 represents front-back reflect -->
0.00001 Gazebo/Grey /xacro:macro

<!--origin xyz="0 0 0" rpy="${pi/2} 0 0"/-->
0.0 0
0.0000001 0.00001 Gazebo/Black <!--Added Transmission Code --> transmissioninterface/SimpleTransmission <actuator name="${prefix}${suffix}wheelmotor"> VelocityJointInterface 1 VelocityJointInterface /xacro:macro

Gazebo/White 0.00001

Gazebo/Blue 30.0 1.3962634 640 480 R8G8B8 0.02 300 gaussian <!-- Noise is sampled independently per pixel on each frame. That pixel's noise value is added to each of its color channels, which at that point lie in the range [0,1]. --> 0.0 0.007 true 0.0 camera1 imageraw camerainfo cameralink 0.07 0.0 0.0 0.0 0.0 0.0 <!--link name="kinectoptical_link"/>

0 0 0.036 0 0 0

<geometry>
  <mesh filename="package://learning_tf/meshes/hokuyo.dae"/>
</geometry>


0 0 0 0 0 0 true 40 720 1 -1.570796 1.570796 0.10 30.0 0.01 gaussian <!-- Noise parameters based on published spec for Hokuyo laser achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and stddev of 0.01m will put 99.7% of samples within 0.03m of the true reading. --> 0.0 0.01 /hokuyo/laser/scan hokuyo_link

/

Find also below attached my diff_drive YAML file.

 type: "diff_drive_controller/DiffDriveController"
publish_rate: 100

left_wheel:   ['left_front_wheel_joint','left_back_wheel_joint'] 
right_wheel:  ['right_front_wheel_joint','right_back_wheel_joint'] 

wheel_separation: 0.50

# Odometry covariances for the encoder output of the robot. These values should
# be tuned to your robot's sample odometry data, but these values are a good place
# to start
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

# Top level frame (link) of the robot description
base_frame_id: base_link

# Velocity and acceleration limits for the robot
linear:
  x:
    has_velocity_limits    : true
    max_velocity           : 0.2   # m/s
    has_acceleration_limits: true
    max_acceleration       : 0.6   # m/s^2
angular:
  z:
    has_velocity_limits    : true
    max_velocity           : 2.0   # rad/s
    has_acceleration_limits: true
    max_acceleration       : 6.0   # rad/s^2 

Note that when I ran this URDF without the diff_drive, all the transforms worked just fine. As soon as I included the diff drive, the TF map changed. As a result of the new link between odom frame and base link frame, my Maps are totally skewed. I get very haphazard maps when running slam-gmapping.

Has anyone experienced this? My information provided may not be as concise as you want... Please ask for clarifications if needed. Thank you.

Asked by afroRoboticist on 2019-04-05 08:27:13 UTC

Comments

Answers

So I found out you can stop gazebo from broadcasting the odom -> Base_footprint transform by going to the launch file where you launch your empty_world or whatever world it is you use and insert the following as an argument


This takes away the gazebo transform. However, this requires you to be entirely responsible for broadcasting the odom->base_footprint transform yourself. Was able to get this done.

For some reason though, the issue with the map not being as good still persists. Hopefully I find a fix soon.

Asked by afroRoboticist on 2019-04-09 06:32:54 UTC

Comments

Hi, I am facing the same problem and I can't create the map, what I did is using the plugin differential_drive_controller instead of gazebo_ros_control, but my robot is not stable. Did you found a solution to use gazebo_ros_control and create the map correctly ? Thanks.

BTW you can check my problem in the following link: https://answers.ros.org/question/328675/laserscan-move-with-robot-in-rviz/

Asked by Youssef_Lah on 2019-07-24 16:27:21 UTC