Combine ICP with PGO for SLAM

[IshaanParikh]

I have the PGO(pose graph optimization) backend ready in cpp in the SLAMv2 repository. We have an ICP implementation in python done before. So I need to understand that and find a way for the 2 programs to work with each other.

[IshaanParikh]

Motor controller for the LiDAR works.

#include "rclcpp/rclcpp.hpp"
#include
#include
#include

class MotorController : public rclcpp::Node {
public:
MotorController() : Node("motor_controller") {
RCLCPP_INFO(this->get_logger(), "Starting RPLiDAR motor on GPIO 18...");

    // Export GPIO 18
    std::ofstream export_file("/sys/class/gpio/export");
    if (export_file.is_open()) {
        export_file << "18";
        export_file.close();
        std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Wait for OS to create the virtual files
    }

    // Set direction to output
    std::ofstream dir_file("/sys/class/gpio/gpio18/direction");
    if (dir_file.is_open()) {
        dir_file << "out";
        dir_file.close();
    }

    // Set pin HIGH (100% speed)
    std::ofstream val_file("/sys/class/gpio/gpio18/value");
    if (val_file.is_open()) {
        val_file << "1";
        val_file.close();
    } else {
        RCLCPP_ERROR(this->get_logger(), "FAILED to open GPIO 18. Check permissions!");
    }
}

~MotorController() {
    RCLCPP_INFO(this->get_logger(), "Stopping RPLiDAR motor...");
    
    // Set pin LOW (Stop motor)
    std::ofstream val_file("/sys/class/gpio/gpio18/value");
    if (val_file.is_open()) {
        val_file << "0";
        val_file.close();
    }
    
    // Unexport GPIO 18 to clean up
    std::ofstream unexport_file("/sys/class/gpio/unexport");
    if (unexport_file.is_open()) {
        unexport_file << "18";
        unexport_file.close();
    }
}

};

int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared());
rclcpp::shutdown();
return 0;
}

[IshaanParikh]

Launch file works, and we are publishing this to Foxglove. Foxglove is a mapping software for LiDAR data which can help visualize what the LiDAR is seeing

[IshaanParikh]

Got a working LiDAR thanks to Erica. It works allegedly, but I cannot get Foxglove to visualize the data. Taking help from Gemini. Current problem seems to be with permissions on the Pi, and some problems in my launch file, so working around them. Shouldn't take too long.

[IshaanParikh]

I had a lot of trouble with CEV router, which meant I couldn't log into any Pi. I tried switching the network to RedRover, my hotspot and usb debugging, but couldn't figure it out. I couldn't do much work on Sunday.

[IshaanParikh]

CEV router is back! And everything is working. However, there is a very weird error. The lidar is spinning, and publishing, but it is not being found. This is really annoying, I am super close to just starting with the IMU. Trying my best to figure this out soon so that I can test PGO :(