Goal reached, incorrect map

asked 2015-05-25 05:58:24 -0500

RND gravatar image

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:

Incorrect map after goal is reached

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 ...
(more)
edit retag flag offensive close merge delete

Comments

What is the orientation of your robot when it starts? And where does it get its orientation from?

yohtm gravatar imageyohtm ( 2015-05-25 11:12:33 -0500 )edit

the orientation of the robot when it starts is (x, y, z) = (0,0,0) and w = 1 (in quaternion form). So it is oriented face forwards and the axis set to base_link show the red axis forward, green axis left. What do you mean where it gets its orientation from? I have gmapping and RosAria.

RND gravatar imageRND ( 2015-05-25 11:31:51 -0500 )edit