Carter gmapping error

Issue: Carter gmapping errors: Issue- Gmapping has errors if the robot wheel moves with an angle to its base.

map.img_00060

Set up: 2019 SDK

I have a custom robot which has a 2 wheel robot base and a body with 2 trailing wheels . Lidar is mounted on to the body of the robot which is connected with a hinge joint to the base. So on the rotation of the robot , there is an angle between robot and the lidar. (hinge joint angle). When I get the hinge angle from unity to my code, I am injecting that between scan and map using my own codelet.
{
“source”: “2d_carter.subgraph/interface/flatscan”,
“target”: “lidar_angle_change_node/lidar_angle_change_codelet/scan”
},
{
“source”: “lidar_angle_change_node/lidar_angle_change_codelet/flatscan”,
“target”: “gmapping/gmapping/flatscan”
},
{
“source”: “lidar_angle_change_node/lidar_angle_change_codelet/flatscan”,
“target”: “local_map.subgraph/interface/flatscan”
},

Just rotation or moving with no wheel angle works fine. Same set up works in simulation with an imported model of the robot.

Lidar angle changing codelet…


for(uint i = 0; i<angles.size();i++) {
float newAngle = angles[i] + encoder_radian;
if (angles[i] <min_angle)
min_angle = angles[i];
if(angles[i] > max_angle)
max_angle = angles[i];
if(newAngle > 2.0fM_PI){
newAngle = newAngle - 2.0f
M_PI;
}else if( newAngle < 0){
newAngle = newAngle + 2.0f*M_PI;
}
changed_angles.set(i, newAngle);
}

These errors can accumulate if your kinematic motion model does not match your robot. For example, the differential drive motion model (two fixed wheels that can turn in opposing directions) will fit poorly with Ackermann drive kinematics (automobile motion with joint steering wheel). This will introduce error into your mapping.

Also, if the sensors are not attached to an articulated joint, you should not be modifying the beam angles from your lidar. Even then, you should have a pose between two frames in the frame graph to account for this.