laser pattern turning along with robot in Rviz

edit

I am having problems with my laser pattern in Rviz.

In my odometry, I checked that:

• When driving 4 meter in a straight line, the number of encoder ticks in both wheels correspond with the calculated number over that distance.
• When driving 1 meter forward, in Rviz the robot also travels approximately 1 grid square
• When turning the robot 90 degrees anticlockwise, in Rviz it also turns approximately 90 degrees anticlockwise

My problem: When I spin the robot around its own Z, the grid and the map stand still in Rviz, but the rplidar laser pattern turns along with the robot. (the red laser pattern behaves as if the entire room turns together with the robot). This problem does not occur when I move the robot in a straight line. Only when I turn.

Result is that when I set out a goal, the robot drives around orientlessly like drunk as the laser pattern is not consistent with the fixed world.

Relevant patameters in my RVIZ:

• Global Options, Fixed Frame: map
• Grid, Reference Frame: Fixed Frame
• Map, Topic: /map
• Local Planner, Topic: /move_base/DWAPlannerROS/local_plan
• LocalCostmap, Topic: /move_base/local_costmap/costmap
• Global Planner, Topic: /move_base/DWAPlannerROS/global_plan
• GlobalCostmap, Topic: /move_base/global_costmap/costmap
• Robot Footprint, Topic: /move_base/local_costmap/footprint
• Path, Topic: move_base/DWAPlannerROS/local_plan

relevant link in my urdf:

 <joint name="base_link_to_laser" type="fixed"> <!-- laser pos relative to origin vehicle -->
   <parent link="base_link"/>
   <child link="laser"/>
   <origin xyz="0.33 0 0.15" rpy = "0 0 3.1415" />
 </joint>

relevant part of my C++ code, using the counter status of both motor encoders:

    //***** update transforms ****             

    ros::Time t = ros::Time::now();
    double dt = (t - tPrev).toSec();

    double dWheelSpinAngle1 = (cnt1 - cnt1Prev) * 2 * M_PI / (EncoderFactor * GearRate);//rad
    double dWheelSpinAngle2 = (cnt2 - cnt2Prev) * 2 * M_PI / (EncoderFactor * GearRate);//rad

    double wheelDisplacement1 = dWheelSpinAngle1 * 0.5 * WheelDiam * DiamtrCorrec1; //m
    double wheelDisplacement2 = dWheelSpinAngle2 * 0.5 * WheelDiam * DiamtrCorrec2; //m
    double dVehicleLin_x = (wheelDisplacement1 + wheelDisplacement2) / 2; //m
    double dVehicleLin_y = 0;// we can't drive sideways
    double dVehicleAng =  (wheelDisplacement1 - wheelDisplacement2) / WheelDistance; //rad

    double vx, vy, vth;
    if (dt > 0)
    {
       vx = dVehicleLin_x / dt;
       vy = 0;// we can't drive sideways
       vth = dVehicleAng / dt;
    }
    else
    {
       vx = 0;
       vy = 0;
       vth = 0;
    }

    //update position transform:
    double x = xPrev + (dVehicleLin_x * cos(thPrev) - dVehicleLin_y * sin(thPrev));//new x pos. 
    double y = yPrev + (dVehicleLin_x * sin(thPrev) + dVehicleLin_y * cos(thPrev));//new y pos. 
    double z = 0;// we can't fly
    double th = thPrev + dVehicleAng; //vehicle's new orientation 

    Transformed Tra;            

    //Trans values:

    //header:
    Tra.odomTrans.header.stamp = t; //current time
    Tra.odomTrans.header.frame_id = "odom"; //world frame
    Tra.odomTrans.child_frame_id = "base_link"; //vehicle frame 

    printf("\ncalculating transforms: \n");

    //displacement:
    Tra.odomTrans.transform.translation.x = x; printf("x: %g ", x);
    Tra.odomTrans.transform.translation.y = y; printf("  y: %g \n\n", y);
    Tra.odomTrans.transform.translation.z = 0.0; 

    //rotation:             
    Tra.odomTrans.transform.rotation = tf::createQuaternionMsgFromYaw(th);   


    //Pose values:

    printf("\ncalculating poses: \n");
    //header:
    Tra.odomPose.header.frame_id = "odom";//topic
    Tra.odomPose.header.stamp = t;
    Tra.odomPose.child_frame_id = "base_link";//vehicle's coordinate frame

    //position:
    Tra ...