Where is the TurtleBot 4?

An ICP localizer in ROS 2, measured against OptiTrack ground truth.

See the measurements Read the code

  • OptiTrack
  • ICP estimate
  • Lab map

1. Know the robot

A TurtleBot 4 Standard, its LiDAR mounted 90° off the driving axis.

Base
iRobot Create 3
On-board computer
Raspberry Pi 4 Model B
LiDAR
RPLIDAR A1, 720 beams per turn
Camera
OAK-D Pro
The TurtleBot 4 switched on, on the lab floor, with three reflective markers fixed above its camera
Three reflective markers above the camera: this is how OptiTrack sees the robot.

2. Measure the laser

Where the beam meets the chassis posts the reading is missing, and at their edges it wavers.

  • Mean distance, in metres
  • Standard deviation
Robot standing still in a square pen, 137 readings per beam. The orange sectors are masked out in the reactive nodes.

3. Lock onto the map

The icp_map node aligns every scan to a 569-point map.

First search
1 m grid, 8 headings
Correspondences
within 0.5 m
Iterations
15 while searching, 50 while tracking
Beams kept
beyond 0.3 m from the sensor
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>::Ptr est(
    new pcl::registration::TransformationEstimation2D<pcl::PointXYZ, pcl::PointXYZ>);
icp.setTransformationEstimation(est);

icp.setInputSource(current_cloud);
icp.setInputTarget(map_cloud_);
icp.setMaxCorrespondenceDistance(0.5);
icp.setMaximumIterations(50);
icp_map.cpp
The RIMLab from above: the TurtleBot at the centre of the floor, with the x and y axes of the reference frame drawn over it

4. Ask OptiTrack for the truth

Twelve infrared cameras track the markers. Standing still, the ICP is off by about 4 cm and 1°.

5. Compare three paths

On the circle, the ICP mean error is 2.39 cm.

A 50 cm radius circle, two laps
ComparisonMean errorσ
OptiTrack against ideal0.30 cm0.32 cm
ICP against ideal1.34 cm1.35 cm
ICP against OptiTrack2.39 cm1.74 cm
  • Ideal
  • OptiTrack
  • ICP
A 1 m square, driven by a timer
ComparisonMean errorσ
OptiTrack against ideal3.87 cm3.56 cm
ICP against ideal5.71 cm4.79 cm
ICP against OptiTrack3.42 cm3.18 cm
Closure, OptiTrack6.78 cm
Closure, ICP8.80 cm
A 1 m square, closed on odometry
ComparisonMean errorσ
OptiTrack against ideal5.19 cm3.98 cm
ICP against ideal7.87 cm5.34 cm
ICP against OptiTrack6.01 cm4.05 cm
Closure, OptiTrack10.30 cm
Closure, ICP10.62 cm

Eight ROS 2 nodes in C++

Open a row to read the part that matters.

  • icp_map

    Localises the robot on the map with planar ICP262 lines
    The global search that locks the first pose
    double step = 1.0;
double test_angles[8] = {0.0, M_PI_4, M_PI_2, 3*M_PI_4, M_PI, -3*M_PI_4, -M_PI_2, -M_PI_4};

// iteriamo su tutta la superficie della stanza
for (double x = min_pt.x; x <= max_pt.x; x += step) {
    for (double y = min_pt.y; y <= max_pt.y; y += step) {
        for (int i = 0; i < 8; ++i) {
            double cos_y = std::cos(test_angles[i]);
            double sin_y = std::sin(test_angles[i]);

            Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
            guess(0, 0) = cos_y;
            guess(0, 1) = -sin_y;
            guess(1, 0) = sin_y;
            guess(1, 1) = cos_y;
            guess(0, 3) = x;
            guess(1, 3) = y;

            pcl::PointCloud<pcl::PointXYZ> temp_final;
            icp.setMaximumIterations(15);
            icp.align(temp_final, guess);

            if (icp.hasConverged()) {
                double score = icp.getFitnessScore();
                if (score < best_score) {
                    best_score = score;
                    best_guess = guess;
                }
            }
        }
    }
}

  • laser_stat

    Mean and standard deviation of every beam79 lines
    The sums accumulated at every scan
    for (int i = 0; i < num_raggi; i++) {
    float distanza = msg->ranges[i];

    if (std::isfinite(distanza)) {
        somme_[i] += distanza;
        somme_quadrati_[i] += (distanza * distanza);
        conteggi_[i]++;
    }
}

  • virtual_bumper

    Stops the robot 0.35 m from an obstacle96 lines
    The front sector and the stop threshold
        if (a_robot > -0.5 && a_robot < 0.5) {
        if (r < min_distance) {
            min_distance = r;
        }
    }
}

if (min_distance < threshold_) {
    cmd.linear.x = 0.0;
    cmd.angular.z = 0.0;
} else {
    cmd.linear.x = speed_;
    cmd.angular.z = 0.0;
}

  • obstacle_avoider

    Turns towards the freer side105 lines
    The choice between left and right
    if (min_front < threshold_) {
    cmd.linear.x = 0.0;
    if (min_left > min_right) {
        cmd.angular.z = turnrate_;
    } else {
        cmd.angular.z = -turnrate_;
    }
} else {
    cmd.linear.x = speed_;
    cmd.angular.z = 0.0;
}

  • quadrato_nodo

    A timed 1 m square, with no feedback88 lines
    The 90° turn, counted in cycles
    // FASE 1: GIRA DI 90 GRADI
else if (fase_ == 1) {
  messaggio.angular.z = 0.785;
  contatore_++;

  // CURVA: 2 secondi (20 cicli)
  if (contatore_ >= 20) {
    contatore_ = 0;
    lati_completati_++;

    if (lati_completati_ >= 4) {
      fase_ = 2; // Ha fatto 4 lati completi
    } else {
      fase_ = 0;
      RCLCPP_INFO(this->get_logger(), "==> PARTENZA LATO %d", lati_completati_ + 1);
    }
  }
}

  • quadrato_odom_nodo

    The same square, closed on odometry95 lines
    The 1 m side measured on /odom
    // FASE 0: VAI DRITTO FINO A 1 METRO
if (fase_ == 0) {
    double distance = std::sqrt(std::pow(current_x - start_x_, 2) + std::pow(current_y - start_y_, 2));

    if (distance < 1.0) {
        cmd.linear.x = 0.15;
        cmd.angular.z = 0.0;
    } else {
        cmd.linear.x = 0.0;
        fase_ = 1;
        init_pose_captured_ = false;
        RCLCPP_INFO(this->get_logger(), "inizio curva %d ", lati_completati_ + 1);
    }
}

  • cerchio_nodo

    A 50 cm radius circle at constant speed45 lines
    Two constant speeds are enough
    void cerchio_robot()
{
  geometry_msgs::msg::Twist messaggio;
  messaggio.linear.x = 0.2;
  messaggio.angular.z = 0.4;
  publisher_->publish(messaggio);
}

  • slalom_nodo

    Alternating turns every two seconds69 lines
    Right and left, four cycles each
    // fase 0 destra
if (fase_==0){
messaggio.linear.x = 0.45;
messaggio.angular.z = 1.2;
cont_=cont_+1;
  if (cont_>=4){
      fase_=1;
      cont_=0;
  }
}