修改时间3

create_robot_describer/src/create_robot_describe.cpp

@@ -13,6 +13,7 @@
 typedef CGAL::Exact_predicates_inexact_constructions_kernel     CgalK;
 typedef CgalK::Point_3                                          CgalPoint;
 namespace fs = std::filesystem;
+using namespace tinyxml2;
 using namespace Eigen;
 
 int RobotDescription::InitJointModel()
@@ -470,6 +471,74 @@ std::string findFileInCurrentDir(const std::string& target_filename, std::string
     return "";
 }
 
+std::string GetWriteFileName(const std::string& dir, const std::string& filename) {
+    try {
+        // 1. 检查并创建目录
+        if (!fs::exists(dir)) {
+            if (!fs::create_directories(dir)) {
+                throw std::runtime_error("无法创建目录：" + dir);
+            }
+            std::cout << "已创建目录：" << dir << std::endl;
+        }
+
+        // 2. 拼接完整文件路径
+        fs::path file_path = fs::path(dir) / filename;
+        std::string full_path = file_path.string();
+        return full_path;
+    } catch (const fs::filesystem_error& e) {
+        throw std::runtime_error("文件系统错误：" + std::string(e.what()));
+    } catch (const std::exception& e) {
+        throw std::runtime_error("操作失败：" + std::string(e.what()));
+    }
+    return "";
+}
+
+void WriteXmlFile(const std::string& file_path, RobotDescription& robot_description) {
+    XMLDocument doc;
+
+    // 添加XML声明（可选，但推荐）
+    XMLDeclaration* decl = doc.NewDeclaration();
+    doc.InsertFirstChild(decl);
+
+    // 创建根节点 <root>
+    XMLElement* root = doc.NewElement("root");
+    doc.InsertEndChild(root);  // 将根节点添加到文档
+
+    // 向根节点添加子节点 <node id="1">
+    XMLElement* node1 = doc.NewElement("node");
+    node1->SetAttribute("id", "1");  // 设置属性
+    root->InsertEndChild(node1);
+
+    // 向node1添加子节点 <param>
+    XMLElement* param1 = doc.NewElement("param");
+    param1->SetAttribute("name", "name");
+    param1->SetAttribute("value", "example");
+    node1->InsertEndChild(param1);
+
+    XMLElement* param2 = doc.NewElement("param");
+    param2->SetAttribute("name", "type");
+    param2->SetAttribute("value", "xml");
+    node1->InsertEndChild(param2);
+
+    // 向根节点添加另一个子节点 <node id="2">
+    XMLElement* node2 = doc.NewElement("node");
+    node2->SetAttribute("id", "2");
+    root->InsertEndChild(node2);
+
+    // 向node2添加文本节点 <value>
+    XMLElement* value = doc.NewElement("value");
+    value->SetText("123.456");  // 设置文本内容
+    node2->InsertEndChild(value);
+
+    // 4. 将XML内容写入文件
+    XMLError error = doc.SaveFile(file_path.c_str());
+    if (error != XML_SUCCESS) {
+        throw std::runtime_error("写入XML文件失败：" + file_path + "（错误码：" + std::to_string(error) + "）");
+    }
+
+    printf("成功写入XML文件: %s\n", file_path.c_str());
+}
+
 int main(int argc, char *argv[])
 {
     UNUSED(argc);
@@ -486,7 +555,8 @@ int main(int argc, char *argv[])
     // 创建RobotDescription对象
     RobotDescription robot_description(urdf_string, srdf_string);
 
-    // 其他逻辑...
+    std::string output_file = GetWriteFileName(urdf_dir, "FHrobot.xml");
 
+    WriteXmlFile(output_file, robot_description);
     return 0;
 }

rm_arm_control/src/rm_arm_node.cpp

@@ -388,14 +388,14 @@ void RmArmNode::ArmActionCheck()
     while (rclcpp::ok()) {
         if (!busy) {
             int result = sem_timedwait(&sem_, &timeout);
-            if (result == 0) {
-                if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) {
-                    StartNewGoal(leftArmHandler_);
-                }
-                if (rightArmHandler_ != NULL && (rightArmHandler_->armStatus == ARM_STATUS_READY)) {
-                    StartNewGoal(rightArmHandler_);
-                }
+
+            if (leftArmHandler_ != NULL && (leftArmHandler_->armStatus == ARM_STATUS_READY)) {
+                StartNewGoal(leftArmHandler_);
             }
+            if (rightArmHandler_ != NULL && (rightArmHandler_->armStatus == ARM_STATUS_READY)) {
+                StartNewGoal(rightArmHandler_);
+            }
+            
         }
         int64_t currentTimeNs = get_clock()->now().nanoseconds();
         if (currentTimeNs >= nextCheckPoint) {