simulation

edit

Hi!
When I ran a simulation in gazebo and I use rviz to send goals what my robot does in rviz is different from what it does in gazebo.
More precisely if I set a goal with the rviz tool 2d nav goal i can see the robot in gazebo moving in a smooth way but not in the right direction to reach the goal and the robot in rviz which change its position in a non-continuous way (e.g. it disappear from (x,y) and reapper in (x2,y2) which is far from the previous position).
Further more while the robot in gazebo does a rotation of k rad the one in rviz does a rotation of h rad (with h < k).
Setting the laser decay time to 10 and teleoperating the robot I can see that the scans are coinciding if I move the robot forward or backward but they're totally not coinciding if I let the robot rotate in place. Instead I was expecting to see the same translation and rotation performed by the robot in gazebo and the robot in rviz. What could be the issue?

What i do is: To launch pioneer3dx.gazebo.launch to simulate the p3dx robot.

<launch>

  <include file="$(find gazebo_worlds)/launch/office_world.launch"/>

  <include file="$(find p2os_urdf)/launch/upload_pioneer3dx.xml"/>

  <!-- push robot_description to factory and spawn robot in gazebo -->
  <node name="spawn_pioneer" pkg="gazebo" type="spawn_model" 
    args="-z 0.051 
          -urdf 
          -param robot_description  
          -model robot_description" 
    respawn="false" output="screen" />

  <!-- Controller Manager -->
  <include file="$(find pr2_controller_manager)/controller_manager.launch" />

  <!-- load controllers -->
  <node name="diffdrive" pkg="gazebo_plugins" type="gazebo_ros_diffdrive" respawn="true" output="screen"/>


</launch>

Then I'm launching the navigation.launch file:

<launch>
    <master auto="start"/>

    <!-- Run the map server (you can run it here or in another terminal) -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find p2os_nav)/simple_office_map.yaml" respawn="true"/> 

    <!--- Run AMCL -->
    <include file="$(find p2os_launch)/amcl.launch" />

    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
        <rosparam file="$(find p2os_launch)/costmap_common_params.yaml" command="load" ns="global_costmap" />
        <rosparam file="$(find p2os_launch)/costmap_common_params.yaml" command="load" ns="local_costmap" />
        <rosparam file="$(find p2os_launch)/local_costmap_params.yaml" command="load" />
        <rosparam file="$(find p2os_launch)/global_costmap_params.yaml" command="load" />
        <rosparam file="$(find p2os_launch)/base_local_planner_params.yaml" command="load" />
        <param name="base_global_planner" type="string" value="navfn/NavfnROS" />
        <param name="conservative_reset_dist" type="double" value="3.0" />
        <param name="controller_frequency" type="double" value="15.0" />
        <remap from="odom" to="erratic_odometry/odom"/>
    </node>


</launch>

and also rviz (with the command rosrun rviz rviz). Surfing on the web i read that it could be a problem of too slow odometry, but what to do to fix it?

The /tf between /odom and /base_link and the nav_msgs odom received by move_base is published by the gazebo diffdrive plugin:

#include <gazebo/gazebo.h>
#include <gazebo/GazeboError.hh>
#include <gazebo/Quatern.hh>

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

#include <boost/bind.hpp>

class DiffDrive {
public:
  libgazebo::PositionIface *posIface;
  ros::NodeHandle* rnh_;
  ros::Subscriber ...