On 09/30/2012 01:09 PM, Claudio Carbone wrote:
-I can't get a working include of another position model inside a 
pioneer, would you mind walking me through it?

Found the solution.
For posterity I'll walk through it.

The main concept in the definition of model inside stage is piggybacking (also known as inheritance).
That's to say you can not explicitly link two different things. No "horizontal" linking.
The only way you can obtain something like that is if you put the second one on top of the first, that way the second moves with the first.

This is clearly visible if you have a look at pioneer.inc: everything new is put "on top" of a pre-existing definition (ex define pioneer2dx_base_no_sonar pioneer_base, that's to say define a new model named pioneer2dx_base_no_sonar taking everything inside pioneer_base and adding what follows between parenthesis).

That's wonderful, but it may give problems when what you're trying to get is absolute parallelism.
That is the case with positioning devices: you want two (or possibly more) different systems to report the position of the same object (the robot).
All sounds nice but there is a small caveat: when a child literally piggybacks on his father, he moves but doesn't really walk at all!

That's to say that the simulated wheels of the secondary position model don't actually get spinned around!
So in order to work around this, you have to be smart: odometry NEEDS its wheels to move to get anything back, while gps doesn't. The solution is to modify pioneer.inc to use the odometry model, and specify a secondary gps position model in the world file as depicted below.

pioneer.inc
define pioneer_base position
(
  color "red"            # Default color.
  drive "diff"             # Differential steering model.
  gui_nose 1                      # Draw a nose on the robot so we can see which way it points
  obstacle_return 1               # Can hit things.
  ranger_return 0.5                # reflects sonar beams
  blob_return 1                   # Seen by blobfinders 
  fiducial_return 1               # Seen as "1" fiducial finders

  #localization "gps"              
  #localization_origin [0 0 0 0]     # Start odometry at (0, 0, 0).

  # alternative odometric localization with simple error model
  localization "odom"                 # Change to "gps" to have impossibly perfect, global odometry
  odom_error [ 0.05 0.05 0.05 0.05]       # Odometry error or slip in X, Y and Theta
                                       # (Uniform random distribution)  
)


file.world
fancypioneer2dx
(         
  # can refer to the robot by this name
  name "r0"
  pose [ -7.5 -7 0 0.000 ]
  color "red"

  # pioneer2dx's sonars    will be ranger:0 and the laser will be ranger:1
  fancysicklaser( pose [ 0 0 0 0 ] )
 
  # demonstrate a plugin controller, implemented in examples/ctrl/wander.cc
  # you probably should comment this out when using simple.cfg with Player
  ctrl "wander"

  position (
    size [0.01 0.01 0.01]
    localization "gps"
    localization_origin [ -7.5 -7 0 0 ]
    #odom_error [ 1 1 1 1 ]

    # [ xmin xmax ymin ymax zmin zmax amin amax ]               
    velocity_bounds [0 0 0 0 0 0 0 0 ]                   
    acceleration_bounds [0 0 0 0 0 0 0 0]
  )
 
)

The gps origin has been moved to have the same zero of the odometry and thus better compare results and drifts.

Hope this helps in the future.
Thank you everybody for your patience.

Regards
Claudio