gmapping Map not updating correctly.

edit

Hi everybody. I am having an issue with gmapping. I have a simulated Rover that is publishing LaserScan messages and odometry data. Basically I am seeing that when I drive my Rover next to an obstacle in a straight line, the occuppied cells created by gmapping (seen in rviz) moves with the Rover. I have followed REP-103 and REP-105 and the "Seting up your robot using tf" as close as I can tell. Sorry for the long post.

For tf frames I have:

geometry_msgs::Quaternion base_quat = tf::createQuaternionMsgFromYaw(0.0);
geometry_msgs::TransformStamped base_trans;
base_trans.header.stamp = current_time;
base_trans.header.frame_id = "odom";
base_trans.child_frame_id = "base_link";
base_trans.transform.translation.x = current_Northing_m;
base_trans.transform.translation.y = current_Easting_m;
base_trans.transform.translation.z = -0.2794;
base_trans.transform.rotation = base_quat;
base_broadcaster.sendTransform(base_trans);

geometry_msgs::Quaternion scan_quat = tf::createQuaternionMsgFromYaw(0.0);
geometry_msgs::TransformStamped scan_trans;
scan_trans.header.stamp = ros::Time::now();
scan_trans.header.frame_id = "base_link";
scan_trans.child_frame_id = "laser";
scan_trans.transform.translation.x = 0.0;
scan_trans.transform.translation.y = 0.0;
scan_trans.transform.translation.z = 0.3556;
scan_trans.transform.rotation = scan_quat;
scan_broadcaster.sendTransform(scan_trans);

And my frame links:

As you can see I am not doing anything with rotation yet.

And here are some pictures of my Simulated Rover in my GUI, along with the output of Rviz When I start my program:

And now when the Rover is driven alongside the Obstacle:

And now when the Rover is driven well past the Obstacle: As you can see, the Map is shifting with the Rover.

And here is some output of the gmapping package:

Odom Pose Theta: 0.000000 MPose Theta: 0.430916
GOT MAP
 Center x: -0.400000 y: -0.400000
 update frame 3624
 update ld=2.91054e-14 ad=0
 Laser Pose= 7.70492 -3 0
 m_count 108
 Average Scan Matching Score=499.238
 neff= 29.9807
 Registering Scans:Done

It is worth mentioning that in rviz, when I drive the Rover forwards, the Red axis on the tf frame is the one moving in the correct direction.