Goal reached, incorrect map

edit

Hello,

I am using move_base to make my robot move a certain distance (x,y) relative to itself. I am using the following code in order to send my goal to move_base:

goal.target_pose.header.frame_id = "/base_link";
    goal.target_pose.header.stamp = ros::Time::now();

    double theta;  // target robot orientation
    theta = 0;

    goal.target_pose.pose.position.x = 1.0; //Send robot to goal +1m along x, and 0.3m along y, z remains the same
    goal.target_pose.pose.position.y = 0.3;
    goal.target_pose.pose.position.z = 0;
    goal.target_pose.pose.orientation.x = 0.000;
    goal.target_pose.pose.orientation.y = 0.000;
    goal.target_pose.pose.orientation.z = sin(theta/2); 
    goal.target_pose.pose.orientation.w = cos(theta/2); 

    ROS_INFO("Sending goal");
    ac.sendGoal(goal);

The issue is that in the physical world, the goal is being reached successfully, as I'm expecting it to be. So, I can see that the robot has moved by 1m forward (x-axis) and 0.3m towards its left (y-axis). The trajectory following is also perfect. My only problem is that the map being built at the same time (by gmapping) is totally incorrect and the orientation of the robot in this map is also a mess:

Now, according to my code, the robot should have moved by 1m forward, 0.3m towards the left and then the final orientation would be at 0 degrees to itself, i.e. it should remain facing forwards. Physically, I can see the robot reaching this goal appropriately and perfectly. But what I see in RVIZ is a completely different story. Here are the parameter files that I am using:

common_costmap_params.yaml:

map_type: costmap
transform_tolerance: 0.7
obstacle_range: 2.5
raytrace_range: 3.0
footprint: [[-0.5, -0.33], [-0.5, 0.33], [0.5, 0.33], [0.5, -0.33]]

inflation_radius: 0.55

observation_sources: laser_scan_sensor

laser_scan_sensor: {sensor_frame: laser, 
                    data_type: LaserScan, 
                    topic: /scan, 
                    marking: true, 
                    clearing: true,
                    inf_is_valid: true}

global_costmap_params.yaml:

global_costmap:
    global_frame: /odom
    robot_base_frame: base_link
    update_frequency: 3.0
    publish_frequency: 10.0
    static_map: true
    #width: 30.0
    #height: 30.0
    #resolution: 0.025
    #origin_x: 0.0
    #origin_y: 0.0

plugins:
  - {name: static,                  type: "costmap_2d::StaticLayer"}
  - {name: inflation,               type: "costmap_2d::InflationLayer"}

local_costmap_params.yaml:

local_costmap:
    global_frame: /odom
    robot_base_frame: base_link
    update_frequency: 3.0
    publish_frequency: 10.0
    static_map: false
    rolling_window: true
    width: 10.0
    height: 10.0
    resolution: 0.05
    origin_x: 0.0
    origin_y: 0.0

plugins:
  - {name: obstacles_laser,           type: "costmap_2d::ObstacleLayer"}
  - {name: inflation,                 type: "costmap_2d::InflationLayer"}

base_local_planner_params.yaml:

TrajectoryPlannerROS:
    # Robot Configuration Parameters
    acc_lim_x: 0.5
    acc_lim_y: 0.5
    acc_lim_theta: 2.0
    max_vel_x: 0.2
    min_vel_x: 0.02
    max_rotational_vel: 1.0
    min_in_place_rotational_vel: 0.4
    escape_vel: -0.1
    holonomic_robot: false
    meter_scoring: true

    # Goal Tolerance Parameters
    xy_goal_tolerance: 0.1
    yaw_goal_tolerance: 0.2
#new addition
    latch_xy_goal_tolerance: false

    # Forward Simulation Parameters
    sim_time: 1.0
    sim_granularity: 0.025
    vx_samples: 3
    vtheta_samples: 20

Here's the launch file for move_base:

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

  <arg name="no_static_map" default="false"/>

  <arg name="base_global_planner" default="navfn/NavfnROS"/>
  <!-- <arg name="base_global_planner" default="carrot_planner/CarrotPlanner"/> -->
  <arg name="base_local_planner" default="base_local_planner/TrajectoryPlannerROS"/>

  <node pkg="move_base" type ...