Make one repository for RPLidar and have changed code from cartograph…

…er_ros

.gitignore

@@ -1,3 +1,5 @@
+vsxmake*
+
 docs/*
 !docs/index.html
 !docs/README.md

backend/src/App.cpp

@@ -3,6 +3,7 @@
 
 #include "oatpp/network/Server.hpp"
 
+#include "cartographer/mapping/2d/"
 #include <iostream>
 
 void run() {

backend/xmake.lua

@@ -9,6 +9,7 @@ set_languages("cxx17")
 add_requires("boost", { configs = {chrono = true} })
 add_requires("fmt", "spdlog")
 add_requires("nlohmann_json")
+add_requires("vcpkg::cartographer", {alias = "cartographer"})
 
 -- C++ Backend API for Svelte App
 add_requires("oatpp", "oatpp-websocket")
@@ -25,6 +26,8 @@ target("backend")
     add_packages("oatpp", "oatpp-websocket")
     add_packages("tl_expected")
 
+    add_packages("cartographer")
+
     before_build_files(function(target)
         local frontend_dir = path.join(os.scriptdir(), "..", "frontend")
         os.execv("pnpm", {"--prefix", frontend_dir, "i"})

lidar/.gitignore

@@ -0,0 +1,8 @@
+# Xmake cache
+.xmake/
+build/
+
+# MacOS Cache
+.DS_Store
+
+

lidar/src/main.cpp

@@ -0,0 +1,93 @@
+#include <cartographer/common/configuration_file_resolver.h>
+#include <cartographer/common/make_unique.h>
+#include <cartographer/mapping/map_builder.h>
+#include <cartographer/mapping/2d/submap_2d.h>
+#include <cartographer/sensor/timed_point_cloud_data.h>
+#include "cartographer/mapping/proto/map_builder_options.pb.h"
+#include "RPLidar.h"
+
+#include <set>
+
+// Edited code from "https://github.com/cartographer-project/cartographer_ros"
+std::tuple<cartographer::mapping::proto::MapBuilderOptions, cartographer::mapping::proto::TrajectoryBuilderOptions> LoadOptions(
+	const std::string& configuration_directory,
+	const std::string& configuration_basename) {
+	auto file_resolver =
+		std::make_unique<cartographer::common::ConfigurationFileResolver>(
+			std::vector<std::string>{configuration_directory});
+	const std::string code =
+		file_resolver->GetFileContentOrDie(configuration_basename);
+	cartographer::common::LuaParameterDictionary lua_parameter_dictionary(
+		code, std::move(file_resolver));
+
+	return std::make_tuple(CreateMapBuilderOptions(&lua_parameter_dictionary),
+		CreateTrajectoryBuilderOptions(&lua_parameter_dictionary));
+}
+
+cartographer::mapping::proto::MapBuilderOptions CreateMapBuilderOptions(
+	::cartographer::common::LuaParameterDictionary* const
+	lua_parameter_dictionary) {
+	return
+		::cartographer::mapping::CreateMapBuilderOptions(
+			lua_parameter_dictionary->GetDictionary("map_builder").get());
+}
+
+cartographer::mapping::proto::TrajectoryBuilderOptions CreateTrajectoryBuilderOptions(
+	::cartographer::common::LuaParameterDictionary* const
+	lua_parameter_dictionary) {
+	return
+		::cartographer::mapping::CreateTrajectoryBuilderOptions(
+			lua_parameter_dictionary->GetDictionary("trajectory_builder").get());
+}
+
+int main() {
+	cartographer::mapping::proto::MapBuilderOptions map_builder_options;
+	cartographer::mapping::proto::TrajectoryBuilderOptions trajectory_builder;
+	std::tie(map_builder_options, trajectory_builder) = LoadOptions("", "");
+
+	cartographer::mapping::MapBuilder map_builder(map_builder_options);
+
+	std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId> sensor_ids;
+
+	sensor_ids.insert(cartographer::mapping::TrajectoryBuilderInterface::SensorId{
+		cartographer::mapping::TrajectoryBuilderInterface::SensorId::SensorType::LOCAL_SLAM_RESULT,
+		"",
+		});
+	const int trajectory_id = map_builder.AddTrajectoryBuilder(sensor_ids, trajectory_builder, [](const int trajectory_id, const ::cartographer::common::Time time,
+		const cartographer::transform::Rigid3d local_pose,
+		::cartographer::sensor::RangeData range_data_in_local,
+		const std::unique_ptr<
+		const ::cartographer::mapping::TrajectoryBuilderInterface::
+		InsertionResult>) {
+		});
+	// Initialize and start your lidar sensor here (e.g., using your_lidar_library).
+	//RPLidar lidar;
+
+	//while (true) {
+	//	// Read lidar data in real-time.
+	//	std::vector<double> lidar_angles;    // List of angles (in radians)
+	//	std::vector<double> lidar_distances; // List of corresponding distances (in meters)
+
+	//	lidar.readData(lidar_angles, lidar_distances);
+
+	//	cartographer::common::Time time = cartographer::common::FromUniversal(0);
+	//	Eigen::Vector3d sensor_pose(0.0, 0.0, 0.0);
+
+	//	cartographer::sensor::TimedPointCloudData point_cloud_data(
+	//		time, sensor_pose, cartographer::sensor::PointCloud{});
+
+	//	for (size_t i = 0; i < lidar_angles.size(); ++i) {
+	//		Eigen::Vector3f point(lidar_distances[i] * std::cos(lidar_angles[i]),
+	//			lidar_distances[i] * std::sin(lidar_angles[i]), 0.0);
+	//		point_cloud_data.point_cloud.push_back(point);
+	//	}
+
+	//	map_builder_options.AddSensorData(trajectory_id, point_cloud_data);
+
+	//	map_builder_options.FinishTrajectory();
+	//	std::this_thread::sleep_for(std::chrono::milliseconds(100));
+	//}
+
+
+	return 0;
+}

lidar/xmake.lua

@@ -0,0 +1,18 @@
+add_rules("mode.debug", "mode.release")
+
+add_requires("vcpkg::cartographer", {alias = "cartographer"})
+
+add_repositories("my-repo repository", {rootdir = path.join(os.scriptdir(), "..")})
+add_requires("rplidar")
+
+target("lidar")
+    set_kind("binary")
+
+    add_packages("cartographer")
+
+    add_packages("rplidar")
+
+    add_files("src/*.cpp")
+
+    add_defines("NOMINMAX")
+    add_defines("GLOG_NO_ABBREVIATED_SEVERITIES")

raspberry_pi/SETUP.md

@@ -1,3 +1,7 @@
 Set up the SSH, Wifi of the Raspberry Pi first.
 
-Run the following command to get XMake: `curl -fsSL https://xmake.io/shget.text | bash`
+Run the following command to get XMake: `curl -fsSL https://xmake.io/shget.text | bash`
+
+Run the following command to get Docker: `curl -fsSL https://get.docker.com -o get-docker.sh && sudo sh get-docker.sh`
+
+Run the following command to get image: `sudo docker pull ros:noetic` and `sudo docker run -it ros:noetic`

repository/packages/r/rplidar/rplidar/include/ExpressPacket.hpp

@@ -0,0 +1,53 @@
+#ifndef EXPRESSPACKET_H
+#define EXPRESSPACKET_H
+
+#pragma once
+
+// Made with the help of ChatGPT
+
+#include <vector>
+#include <tuple>
+#include <stdexcept>
+
+class ExpressPacket {
+public:
+    static const uint8_t sync1 = 0xa;
+    static const uint8_t sync2 = 0x5;
+
+    ExpressPacket(std::vector<uint8_t> data) {
+        if ((data[0] >> 4) != sync1 || (data[1] >> 4) != sync2) {
+            throw std::invalid_argument("try to parse corrupted data");
+        }
+
+        uint8_t checksum = 0;
+        for (size_t i = 2; i < data.size(); i++) {
+            checksum ^= data[i];
+        }
+
+        if (checksum != ((data[0] & 0x0F) + ((data[1] & 0x0F) << 4))) {
+            throw std::invalid_argument("Invalid checksum");
+        }
+
+        new_scan = (data[2] >> 7) & 0x01;
+        start_angle = static_cast<float>((data[1] & 0x0F) << 8 | data[2]) / 64.0f;
+
+        for (size_t i = 4; i < data.size(); i += 5) {
+            distance.push_back(((data[i + 1] >> 2) & 0x3F) | ((data[i] & 0x3F) << 6));
+            angle.push_back((((data[i + 3] & 0x0F) + ((data[i + 1] & 0x01) << 4)) / 8.0f) * getSign(data[i + 1]));
+            distance.push_back(((data[i + 2] >> 2) & 0x3F) | ((data[i + 1] & 0x3F) << 6));
+            angle.push_back((((data[i + 3] >> 4) & 0x0F) + ((data[i + 2] & 0x01) << 4)) / 8.0f * getSign(data[i + 2]));
+        }
+    }
+
+    static int getSign(uint8_t value) {
+        return (value & 0x02) ? -1 : 1;
+    }
+
+public:
+    std::vector<uint16_t> distance;
+    std::vector<float> angle;
+    bool new_scan;
+    float start_angle;
+};
+
+#endif

repository/packages/r/rplidar/rplidar/include/RPLidar.h

@@ -0,0 +1,168 @@
+#ifndef RPLIDAR_H
+#define RPLIDAR_H
+
+#pragma once
+
+// Made with the help of ChatGPT
+
+#include <string>
+#include <stdint.h>
+
+#include <serial/serial.h>
+
+#include <iostream>
+#include <map>
+#include <vector>
+
+// std::this_thread::sleep_for(std::chrono::milliseconds(1));
+#include <chrono>
+#include <thread>
+
+// convertToHexString
+#include <sstream>
+#include <iomanip>
+
+// iter_measures
+#include <functional>
+
+#include <spdlog/spdlog.h>
+#include <spdlog/fmt/bin_to_hex.h>
+
+#include "ExpressPacket.hpp"
+
+#define SYNC_BYTE 0xA5
+#define SYNC_BYTE2 0x5A
+
+#define GET_INFO_BYTE 0x50
+#define GET_HEALTH_BYTE 0x52
+
+#define STOP_BYTE 0x25
+#define RESET_BYTE 0x40
+
+enum ScanType
+{
+    NORMAL,
+    FORCE,
+    EXPRESS
+};
+
+struct ScanInfo
+{
+    int currently_scanning;
+    int dsize;
+    ScanType type;
+};
+
+static std::map<ScanType, std::map<std::string, uint8_t>> SCAN_TYPE = {
+    {ScanType::NORMAL, {{"byte", 0x20}, {"response", 129}, {"size", 5}}},
+    {ScanType::FORCE, {{"byte", 0x21}, {"response", 129}, {"size", 5}}},
+    {ScanType::EXPRESS, {{"byte", 0x82}, {"response", 130}, {"size", 84}}}};
+
+#define DESCRIPTOR_LEN 7
+#define INFO_LEN 20
+#define HEALTH_LEN 3
+
+#define INFO_TYPE 4
+#define HEALTH_TYPE 6
+
+#define MAX_MOTOR_PWM 1023
+#define DEFAULT_MOTOR_PWM 660
+#define SET_PWM_BYTE 0xF0
+
+static std::map<int, std::string> HEALTH_STATUSES = {
+    {0, "Good"},
+    {1, "Warning"},
+    {2, "Error"}};
+
+struct DeviceInfo
+{
+    uint8_t model;
+    std::pair<uint8_t, uint8_t> firmware;
+    uint8_t hardware;
+    std::string serialNumber;
+};
+
+/**
+ * @brief
+ * Health Info for Lidar Scanner
+ */
+struct HealthInfo
+{
+    /**
+     * @brief
+     * 'Good', 'Warning' or 'Error' statuses
+     */
+    std::string status;
+    /**
+     * @brief
+     * The related error code that caused a warning/error.
+     */
+    int errorCode;
+};
+
+struct Measure
+{
+    bool newScan;
+    int quality;
+    double angle;
+    double distance;
+};
+
+/**
+ * @brief Class for communicating with RPLidar rangefinder scanners
+ * 
+ */
+class RPLidar
+{
+public:
+    RPLidar(const std::string &port, uint32_t baudrate = 115200U);
+
+    ~RPLidar();
+
+    void disconnect();
+
+    void _set_pwm(int pwm);
+
+    void set_motor_speed(int pwm);
+
+    void start_motor();
+
+    void stop_motor();
+
+    void _send_payload_cmd(uint8_t cmd, const std::string &payload);
+
+    void _send_cmd(uint8_t cmd);
+
+    std::tuple<uint8_t, bool, uint8_t> _read_descriptor();
+
+    std::vector<uint8_t> _read_response(int dsize);
+
+    DeviceInfo get_info();
+
+    HealthInfo get_health();
+
+    void clean_input();
+
+    void stop();
+
+    void start(ScanType scanType);
+
+    void reset();
+
+    std::function<Measure()> iter_measures(ScanType scanType = NORMAL, int maxBufMeas = 3000);
+
+    std::function<std::vector<Measure>()> iter_scans(ScanType scanType = NORMAL, int maxBufMeas = 3000, int minLen = 5);
+
+private:
+    std::unique_ptr<serial::Serial> _serial = nullptr;
+    std::string port;
+    uint32_t baudrate;
+    int _motor_speed = DEFAULT_MOTOR_PWM;
+    bool motor_running = false;
+    ScanInfo scanning = {false, 0, ScanType::NORMAL};
+    int express_trame = 32;
+    std::unique_ptr<ExpressPacket> express_data = nullptr;
+    std::unique_ptr<ExpressPacket> express_old_data = nullptr;
+};
+
+#endif