Navigation stack problem [closed]

edit

Hi everyone,
I'm using the navigation stack of the p2os package with Gazebo.
I'm launching 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" />
    </node>


</launch>

and also rviz (with the command rosrun rviz rviz).

But when I use the 2D Nav Goal tool of rviz to send a goal message to the robot I can see that the global path shown at first is correct, but the robot doesn't follow it, it starts to navigate in a random way and never stops. And what the robot does in Gazebo is different from what it does in rviz.

These are my configuration files:

BASE LOCAL PLANNER

TrajectoryPlannerROS:
  max_vel_x: 0.8
  min_vel_x: 0.1
  max_rotational_vel: 0.8
  min_in_place_rotational_vel: 0.3
  escape_vel: -0.2

  sim_time: 2.0
  path_distance_bias: 0.6
  goal_distance_bias: 0.6

  acc_lim_th: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5

  holonomic_robot: false

COSTMAP COMMON PARAMS

obstacle_range: 2.5
raytrace_range: 3.0
inflation_radius: 0.5

#---standard pioneer footprint---

#---(in inches)---
#footprint: [ [10, -2], [7, -2], [7, -7], [-7.5, -7], [-10, 0], [-7.5, 7], [7, 7], [7, 2], [10, 2] ]

#---(in meters)---
#footprint: [ [0.254, -0.0508], [0.1778, -0.0508], [0.1778, -0.1778], [-0.1905, -0.1778], [-0.254, 0], [-0.1905, 0.1778], [0.1778, 0.1778], [0.1778, 0.0508], [0.254, 0.0508] ]

#---pioneer AT footprint:---

#---(in inches)---
#footprint: [ [13, -2], [10, -2], [10, -10], [-10, -10], [-10, 10], [10, 10], [10, 2], [13, 2] ]

#---(in meters)---
footprint: [ [0.3302, -0.0508], [0.254, -0.0508], [0.254, -0.254], [-0.254, -0.254], [-0.254, 0.254], [0.254, 0.254], [0.254, 0.0508], [0.3302, 0.0508] ]

transform_tolerance: 0.2
map_type: costmap

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: /laser_base_link, data_type: LaserScan, topic: base_scan/scan, marking: true, clearing: true, expected_update_rate: 0.2}

#point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true ...