Initial commit

src/RobotManager.cpp

@@ -0,0 +1,167 @@
+#include "RobotManager.h"
+#include <sstream>
+#include <iomanip>
+#include <iostream>
+#include <functional>
+
+// 初始化静态成员
+std::unordered_map<std::string, std::shared_ptr<Robot>> RobotManager::_kinematics_table;
+std::unordered_map<std::string, std::string> RobotManager::_urdf_hashes;
+std::recursive_mutex RobotManager::_lock;
+
+// 哈希函数
+std::string calculateHash(const std::string& input) {
+    // 使用标准库的哈希函数组合
+    std::hash<std::string> hasher;
+    size_t h1 = hasher(input);
+    size_t h2 = hasher(input + "salt"); // 添加盐值增加唯一性
+
+    std::stringstream ss;
+    ss << std::hex << h1 << h2;
+    return ss.str();
+}
+
+bool RobotManager::_validateKinematics(const std::shared_ptr<Robot>& robot) {
+    if (!robot || !robot->isInitialized()) {
+        return false;
+    }
+
+    try {
+        // 简单的验证：检查正向运动学计算
+        double test_joints[6] = { 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
+        double tcp_pose[7];
+
+        return robot->calculateFK_TCP(test_joints, tcp_pose);
+    }
+    catch (const std::exception& e) {
+        std::cerr << "Robot validation failed: " << e.what() << std::endl;
+        return false;
+    }
+}
+
+std::pair<bool, std::string> RobotManager::initRobot(const std::string& urdf_robot,
+    const std::string& robot_uuid,
+    bool force_update) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    try {
+        // 创建机器人实例
+        auto robot = std::make_shared<Robot>();
+
+        // 初始化机器人
+        if (!robot->initRobot(urdf_robot)) {
+            return std::make_pair(false, "Failed to initialize robot from URDF");
+        }
+
+        std::string actual_uuid = robot_uuid;
+        if (actual_uuid.empty()) {
+            // 如果没有提供UUID，使用URDF内容的哈希作为UUID
+            actual_uuid = calculateHash(urdf_robot);
+        }
+
+        // 计算URDF哈希
+        std::string urdf_hash = calculateHash(urdf_robot);
+
+        // 检查是否需要更新
+        if (!force_update && _urdf_hashes.find(actual_uuid) != _urdf_hashes.end() &&
+            _urdf_hashes[actual_uuid] == urdf_hash) {
+            return std::make_pair(true, "Content unchanged, skipping update");
+        }
+
+        // 验证运动学
+        if (!_validateKinematics(robot)) {
+            return std::make_pair(false, "URDF validation failed");
+        }
+
+        // 更新表
+        _kinematics_table[actual_uuid] = robot;
+        _urdf_hashes[actual_uuid] = urdf_hash;
+
+        bool existed = (_urdf_hashes.find(actual_uuid) != _urdf_hashes.end());
+        std::string action = existed ? "updated" : "added";
+        return std::make_pair(true, "Successfully " + action + " robot " + actual_uuid);
+    }
+    catch (const std::exception& e) {
+        return std::make_pair(false, "Operation failed: " + std::string(e.what()));
+    }
+}
+
+std::unordered_map<std::string, std::pair<bool, std::string>>
+RobotManager::batchInit(const std::unordered_map<std::string, std::string>& robot_specs) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    std::unordered_map<std::string, std::pair<bool, std::string>> results;
+
+    // 使用传统的迭代器语法
+    for (auto it = robot_specs.begin(); it != robot_specs.end(); ++it) {
+        results[it->first] = initRobot(it->second, it->first);
+    }
+
+    return results;
+}
+
+std::shared_ptr<Robot> RobotManager::getRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    auto it = _kinematics_table.find(robot_uuid);
+    if (it != _kinematics_table.end()) {
+        return it->second;
+    }
+    return nullptr;
+}
+
+std::pair<bool, std::string> RobotManager::removeRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    if (_kinematics_table.find(robot_uuid) == _kinematics_table.end()) {
+        return std::make_pair(false, "Robot " + robot_uuid + " does not exist");
+    }
+
+    _kinematics_table.erase(robot_uuid);
+    _urdf_hashes.erase(robot_uuid);
+
+    return std::make_pair(true, "Successfully removed robot " + robot_uuid);
+}
+
+std::unordered_map<std::string, std::string> RobotManager::listRobots(bool detail) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    std::unordered_map<std::string, std::string> result;
+
+    if (!detail) {
+        for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
+            result[it->first] = "Robot Instance";
+        }
+        return result;
+    }
+
+    for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
+        const std::string& uuid = it->first;
+        const auto& robot = it->second;
+
+        std::stringstream info;
+        info << "Joints: " << robot->getNumberOfJoints() << ", "
+            << "Status: " << (_validateKinematics(robot) ? "valid" : "invalid") << ", "
+            << "Hash: " << _urdf_hashes.at(uuid).substr(0, 8) + "...";
+
+        result[uuid] = info.str();
+    }
+
+    return result;
+}
+
+size_t RobotManager::getRobotCount() {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    return _kinematics_table.size();
+}
+
+void RobotManager::clearAll() {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    _kinematics_table.clear();
+    _urdf_hashes.clear();
+}
+
+bool RobotManager::containsRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    return _kinematics_table.find(robot_uuid) != _kinematics_table.end();
+}