控制节点不再依赖moveit

rm_arm_control/CMakeLists.txt

@@ -52,20 +52,20 @@ find_package(trajectory_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(gjk_interface REQUIRED)
 find_package(interfaces REQUIRED)
-find_package(moveit_core REQUIRED)
-find_package(moveit_ros_planning_interface REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(TINYXML2 REQUIRED tinyxml2)
 
 include_directories(
   include
   ${CMAKE_CURRENT_SOURCE_DIR}/../include
   ${EIGEN3_INCLUDE_DIRS}  # 引入Eigen3的头文件路径
+  ${TINYXML2_INCLUDE_DIR}
 )
 
 # 设置源文件
 set(SOURCES
     src/rm_arm_handler.cpp
     src/rm_arm_node.cpp
-    simulator/rm_arm_simulator.cpp
     simulator/trapezoidal_trajectory_kinematics.cpp
     simulator/rm_driver_kinematics.cpp
     simulator/col_simulator.cpp
@@ -88,8 +88,6 @@ ament_target_dependencies(rm_arm_control
   control_msgs
   gjk_interface
   interfaces
-  moveit_core
-  moveit_ros_planning_interface
 )
 
 # 查找库
@@ -97,6 +95,7 @@ find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/Robot
 
 # 链接库
 target_link_libraries(rm_arm_control ${RMAN_API_LIB})
+target_link_libraries(rm_arm_control ${TINYXML2_LIBRARIES})
 
 # 如果是 UNIX 平台且不是 ARM 平台，确保动态库文件可以在运行时被找到（这一步通常不是必须的，但在某些配置下可能需要）
 if(UNIX)

rm_arm_control/include/col_simulator.hpp

@@ -3,9 +3,9 @@
 
 #include <unordered_map>
 #include <map>
+#include <rclcpp/rclcpp.hpp>
 #include <urdf_model/pose.h>
 #include <std_msgs/msg/string.hpp>
-#include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
 #include "arm_define.h"
 #include "arm_driver_define.hpp"
 #include "gjk_interface.hpp"

rm_arm_control/package.xml

@@ -12,8 +12,6 @@
   <depend>gjk_interface</depend>
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>
-  <exec_depend>moveit_core</exec_depend>
-  <exec_depend>moveit_ros_planning_interface</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav_msgs</exec_depend>

rm_arm_control/simulator/col_simulator.cpp

@@ -1,7 +1,7 @@
 #include <vector>
 #include <string>
 #include <rclcpp/rclcpp.hpp>
-#include <moveit/robot_model/robot_model.h>
+#include <tinyxml2.h>
 #include "col_simulator.hpp"
 
 using namespace Eigen;
@@ -19,10 +19,26 @@ int g_linkStartIndex[BODY_CNT] = {0};
 
 #define POLYHEDRONS_POINTS_CNT   (8)
 
+double truncate_to_4_decimal(double num) {
+    double int_part; // 存储整数部分
+    double frac_part = std::modf(num, &int_part); // 分离小数部分
+
+    // 截断小数部分到前 4 位
+    double truncated_frac = std::trunc(frac_part * 10000.0) * 0.0001;
+
+    // 合并整数部分和截断后的小数部分
+    return int_part + truncated_frac;
+}
+
 static double toRadians(float degrees) {
     return (static_cast<double>(degrees)) * (M_PI / 180.0);
 }
 
+enum
+  {
+    UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
+  } JointTypeE;
+
 int CollisionSimulator::InitJoint()
 {
     const std::map<std::string, int> jointNameToIndexMap = {
@@ -46,7 +62,7 @@ int CollisionSimulator::InitJoint()
         {"right_front_wheel_joint", BODY_TYPE_OTHERS}
     };
     for (JointsInfo &joint : jointMap_) {
-        if (joint.type != urdf::Joint::FIXED) {
+        if (joint.type != FIXED) {
             int jointAngleIndex = jointNameToIndexMap.at(joint.name);
             int bodyType = jointNameToBodyType.at(joint.name);
             for (int i = 0; i < COLLISION_CHECK_SIMPLE_CNT; i++) {
@@ -455,10 +471,10 @@ void CollisionSimulator::UpdatePolyhedrons(int bodyType, int simple)
             CollisionStructureInfo *nowChild = &collision_structures_[linkIndex];
 
             offset = offset + rotation * jointInfo->nextLinkOffset;
-            if (jointInfo->type == urdf::Joint::REVOLUTE) {
+            if (jointInfo->type == REVOLUTE) {
                 double angleRad = toRadians(jointInfo->angle[simple][0]);
                 rotation = AngleAxisd(angleRad, rotation * jointInfo->axis) * rotation;
-            } else if (jointInfo->type == urdf::Joint::PRISMATIC) {
+            } else if (jointInfo->type == PRISMATIC) {
                 double displacement = jointInfo->angle[simple][0];
                 offset = offset + rotation * jointInfo->axis * displacement;
             }