zhengjie/hivecore_robot_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改常量枚举

Browse Source
This commit is contained in:
zj
2025-10-23 18:24:44 +08:00
parent 6b58e7d18e
commit eb07148be1
6 changed files with 73 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
arm_keyboard_input/CMakeLists.txt
View File

@@ -8,7 +8,7 @@ add_compile_options(-g)
include_directories(
include
../../include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
)

38
arm_keyboard_input/src/arm_keyboard_node.cpp
View File

@@ -365,9 +365,9 @@ void ArmKeyboardNode::SendDirectSteppingGoal2(const float *values, int armId)
int64_t timeStamp = get_clock()->now().nanoseconds();
auto goal_msg = ArmAction::Goal();
commandId++;
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.body_id.body_id = armId;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length.data_length = POSE_DIMENSION;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
@@ -389,9 +389,9 @@ void ArmKeyboardNode::SendDirectMovingGoal(const float *values, int armId)
int64_t timeStamp = get_clock()->now().nanoseconds();
auto goal_msg = ArmAction::Goal();
commandId++;
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.body_id.body_id = armId;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制
goal_msg.data_length.data_length = POSE_DIMENSION;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
@@ -414,9 +414,9 @@ void ArmKeyboardNode::SendAngleMovingGoal(const float *values, int armId)
auto goal_msg = ArmAction::Goal();
commandId++;
RCLCPP_INFO(this->get_logger(), "SendAngleMovingGoal id=%ld", commandId);
goal_msg.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.body_id.body_id = armId;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制
goal_msg.data_length.data_length = USED_ARM_DOF;
goal_msg.frame_time_stamp = timeStamp;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {
@@ -648,8 +648,8 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command)
speedadd = -0.01;
}
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length = POSE_DIMENSION;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length.data_length = POSE_DIMENSION;
goal_msg.command_id = commandId;
for (int i = 0; i < POSE_DIMENSION; ++i) {
if (command[directCommandMap[i]]) {
@@ -662,7 +662,7 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command)
if (command[12] && leftDirectCommand_!= commandValueBitMap) {
commandId++;
goal_msg.body_id = LEFT_ARM;
goal_msg.body_id.body_id = LEFT_ARM;
leftDirectCommand_ = commandValueBitMap;
if (leftArmGoalHandle_ != nullptr && leftCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel left arm goal. command id: %ld", leftCommandId_);
@@ -682,7 +682,7 @@ void ArmKeyboardNode::SendDirectSteppingGoal(const char *command)
}
} else if (command[13] && rightDirectCommand_!= commandValueBitMap) {
commandId++;
goal_msg.body_id = RIGHT_ARM;
goal_msg.body_id.body_id = RIGHT_ARM;
rightDirectCommand_ = commandValueBitMap;
if (rightArmGoalHandle_ != nullptr && rightCommandId_ != 0) {
RCLCPP_INFO(this->get_logger(), "cancel right arm goal. command id: %ld", rightCommandId_);
@@ -780,9 +780,9 @@ void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) {
auto goal_msg = ArmAction::Goal();
commandId++;
leftCommandId_ = commandId;
goal_msg.body_id = LEFT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.body_id.body_id = LEFT_ARM;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length.data_length = USED_ARM_DOF;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {
goal_msg.data_array.push_back(leftArmAngleAim_[i]);
@@ -802,9 +802,9 @@ void ArmKeyboardNode::SendAngleSteppingGoal(const char *command) {
auto goal_msg = ArmAction::Goal();
commandId++;
rightCommandId_ = commandId;
goal_msg.body_id = RIGHT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.body_id.body_id = RIGHT_ARM;
goal_msg.data_type.action_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length.data_length = USED_ARM_DOF;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {

7
gjk_interface/src/gjk_interface.cpp
View File

@@ -213,9 +213,10 @@ namespace gjk_interface {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
Vector3d crossAxis = obb1.axis[i].cross(obb2.axis[j]);
if (crossAxis.dot(crossAxis) > 1e-6) { // 排除零向量（轴平行时叉积为零）
auto [aMin, aMax] = projectOBB(obb1, crossAxis.normalized());
auto [bMin, bMax] = projectOBB(obb2, crossAxis.normalized());
if (crossAxis.dot(crossAxis) > 1e-6) { // 排除零向量（轴平行时叉积为零
Vector3d crossAxisNormalized = crossAxis.normalized();
auto [aMin, aMax] = projectOBB(obb1, crossAxisNormalized);
auto [bMin, bMax] = projectOBB(obb2, crossAxisNormalized);
if (!areProjectionsOverlapping(aMin, aMax, bMin, bMax)) {
// 找到分离轴，判定为不碰撞
return false;

3
rm_arm_control/CMakeLists.txt
View File

@@ -50,6 +50,7 @@ find_package(moveit_ros_planning_interface REQUIRED)
include_directories(
include
${CMAKE_CURRENT_SOURCE_DIR}/../include
${EIGEN3_INCLUDE_DIRS} # 引入Eigen3的头文件路径
)
@@ -89,8 +90,6 @@ find_library(RMAN_API_LIB NAMES api_cpp PATHS "${CMAKE_CURRENT_SOURCE_DIR}/Robot
# 链接库
target_link_libraries(rm_arm_control ${RMAN_API_LIB})
# 包含目录
target_include_directories(rm_arm_control PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/../../include")
# 如果是 UNIX 平台且不是 ARM 平台，确保动态库文件可以在运行时被找到（这一步通常不是必须的，但在某些配置下可能需要）
if(UNIX)

51
rm_arm_control/simulator/rm_arm_simulator.cpp
View File

@@ -110,6 +110,8 @@ std::vector<Vector3d> g_dir = {
Vector3d::UnitX(), // R6
};
static int CheckCollisionInner(std::vector<OBB> *addedOBBs, std::vector<OBB> *addedOBBsNext_);
static void InitArmSinglePolyhedron(ArmSimulator *simulator, int index, Polyhedron *poly,
Vector3d &origin, const Vector3d &offset, const Matrix3d &rotation)
{
@@ -163,13 +165,21 @@ static void InitArmPolyhedronsList(ArmSimulator *simulator)
for (int simple = 0; simple < ARM_COLLISION_CHECK_SIMPLE_CNT; simple++) {
std::vector<Polyhedron> *armPolyhedrons = &(simulator->armPolyhedrons_[simple]);
Vector3d offset_temp = {0, 0 ,0};
Vector3d origin_point_offset = {0, 0 ,0};
if (simulator->collision_structures_[BASE].geometry_type == "box") {
origin_point_offset = Vector3d(-simulator->collision_structures_[BASE].dimensions[0] / 2,
-simulator->collision_structures_[BASE].dimensions[1] / 2,
-simulator->collision_structures_[BASE].dimensions[2] / 2);
} else if (simulator->collision_structures_[BASE].geometry_type == "mesh") {
origin_point_offset = Vector3d(simulator->collision_structures_[BASE].mesh_bounds[0],
simulator->collision_structures_[BASE].mesh_bounds[2],
simulator->collision_structures_[BASE].mesh_bounds[4]);
}
Matrix3d rotation = Matrix3d::Identity();
InitArmSinglePolyhedron(simulator, BASE, &(*armPolyhedrons)[BASE],
offset_temp,
Vector3d(-simulator->collision_structures_[BASE].dimensions[0] / 2,
-simulator->collision_structures_[BASE].dimensions[1] / 2,
-simulator->collision_structures_[BASE].dimensions[2] / 2),
rotation);
origin_point_offset,
rotation);
simulator->addedOBBs_[simple][BASE] = gjk_interface::createOBBFromVertices((*armPolyhedrons)[BASE],
simulator->collision_structures_[BASE].dimensions[0],
simulator->collision_structures_[BASE].dimensions[1],
@@ -199,7 +209,7 @@ static void InitArmPolyhedronsList(ArmSimulator *simulator)
simulator->collision_structures_[i].dimensions[2]);
}
}
/*for (int i = LEFT; i <= BASE; ++i) {
for (int i = LEFT; i <= BASE; ++i) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i);
for (int j = 0; j < ARM_POLYHEDRONS_POINT; ++j) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
@@ -208,7 +218,7 @@ static void InitArmPolyhedronsList(ArmSimulator *simulator)
simulator->armPolyhedrons_[0][i][j].z());
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------");
}*/
}
}
static int RemakeCollisionStructureList(ArmSimulator *simulator,
@@ -304,19 +314,15 @@ static void UpdateArmPolyhedrons(ArmSimulator *simulator)
simulator->collision_structures_[i].dimensions[2]);
}
/*RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "last left object:");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i,
simulator->armPolyhedrons_[simple][0][i].x(),
simulator->armPolyhedrons_[simple][0][i].y(),
simulator->armPolyhedrons_[simple][0][i].z());
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "last right object:");
for (int i = 0; i < ARM_POLYHEDRONS_POINT; ++i) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", i,
simulator->armPolyhedrons_[simple][6][i].x(),
simulator->armPolyhedrons_[simple][6][i].y(),
simulator->armPolyhedrons_[simple][6][i].z());
/*for (int i = LEFT; i <= BASE; ++i) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Link %d polyhedron points:", i);
for (int j = 0; j < ARM_POLYHEDRONS_POINT; ++j) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "%d: %.4f %.4f %.4f", j,
simulator->armPolyhedrons_[0][i][j].x(),
simulator->armPolyhedrons_[0][i][j].y(),
simulator->armPolyhedrons_[0][i][j].z());
}
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "---------------------");
}
for (int i = LEFT; i <= BASE; i++) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "OBB: center(%.4f, %.4f, %.4f), axis1(%.4f, %.4f, %.4f), axis2(%.4f, %.4f, %.4f), axis3(%.4f, %.4f, %.4f), "
@@ -609,7 +615,7 @@ static int InitCollisionStructure(urdf::Model &urdf_model, ArmSimulator *simulat
link->collision->origin.rotation.z
};
}
bool valid = true;
// 获取几何形状和尺寸
if (link->collision->geometry) {
switch (link->collision->geometry->type) {
@@ -639,6 +645,7 @@ static int InitCollisionStructure(urdf::Model &urdf_model, ArmSimulator *simulat
auto index = g_linkMap.find(link_name);
if (index == g_linkMap.end()) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "no link");
valid = false;
break;
}
int key = 0;
@@ -661,7 +668,9 @@ static int InitCollisionStructure(urdf::Model &urdf_model, ArmSimulator *simulat
break;
}
}
collision_structures_temp.push_back(col_struct);
if (valid) {
collision_structures_temp.push_back(col_struct);
}
}
}
return 0;

33
rm_arm_control/src/rm_arm_node.cpp
View File

@@ -13,7 +13,7 @@ namespace ArmCommandHash
static int AngleStepOn(RmArmHandler *handle, const ArmGoalSharedPtr goal)
{
if (goal->data_length != USED_ARM_DOF) {
if (goal->data_length.data_length != USED_ARM_DOF) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleStepOn data length error.");
return -1;
}
@@ -30,7 +30,7 @@ namespace ArmCommandHash
static int AngleDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != USED_ARM_DOF) {
if (goal->data_length.data_length != USED_ARM_DOF) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "AngleDirectMove data length error.");
return -1;
}
@@ -48,7 +48,7 @@ namespace ArmCommandHash
static int PoseStepOn(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != POSE_DIMENSION) {
if (goal->data_length.data_length != POSE_DIMENSION) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseStepOn data length error.");
return -1;
}
@@ -61,7 +61,7 @@ namespace ArmCommandHash
static int PoseDirectMove(RmArmHandler *handle, std::shared_ptr<const ArmAction::Goal> goal)
{
if (goal->data_length != POSE_DIMENSION) {
if (goal->data_length.data_length != POSE_DIMENSION) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "PoseDirectMove data length error.");
return -1;
}
@@ -193,11 +193,11 @@ rclcpp_action::GoalResponse RmArmNode::ArmActionGoal(
{
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal request.");
// Accept the goal
if (ArmCommandHash::armCommandMap.find(goal->data_type) == ArmCommandHash::armCommandMap.end()) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type);
if (ArmCommandHash::armCommandMap.find(goal->data_type.action_type) == ArmCommandHash::armCommandMap.end()) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received goal with invalid command id: %d", goal->data_type.action_type);
return rclcpp_action::GoalResponse::REJECT;
}
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id);
if (handle == NULL) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
return rclcpp_action::GoalResponse::REJECT;
@@ -206,7 +206,7 @@ rclcpp_action::GoalResponse RmArmNode::ArmActionGoal(
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
return rclcpp_action::GoalResponse::REJECT;
}
int result = ArmCommandHash::armCommandMap.at(goal->data_type)(handle, goal);
int result = ArmCommandHash::armCommandMap.at(goal->data_type.action_type)(handle, goal);
if (result != 0) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Failed to process the goal.");
return rclcpp_action::GoalResponse::REJECT;
@@ -221,7 +221,7 @@ rclcpp_action::CancelResponse RmArmNode::ArmActionCancel(
int64_t index = goal->command_id;
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Received request to cancel goal. Index: %ld", index);
int result = 0;
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id);
if (handle == NULL) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
return rclcpp_action::CancelResponse::REJECT;
@@ -250,7 +250,7 @@ void RmArmNode::ArmActionFeedback(int coll)
for (auto it = goal_handles_.begin(); it != goal_handles_.end(); ++it) {
auto goal_handle = it->second;
ArmGoalSharedPtr goal = goal_handle->get_goal();
RmArmHandler *handle = GetArmHandler(this, goal->body_id);
RmArmHandler *handle = GetArmHandler(this, goal->body_id.body_id);
if (handle == NULL) {
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "arm validation error.");
continue;
@@ -259,12 +259,12 @@ void RmArmNode::ArmActionFeedback(int coll)
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Arm in error status.");
continue;
}
float *pose = (goal->body_id == LEFT_ARM) ? leftPose : rightPose;
float *pose = (goal->body_id.body_id == LEFT_ARM) ? leftPose : rightPose;
auto feedback = std::make_shared<ArmAction::Feedback>();
feedback->command_id = goal->command_id;
feedback->int_lenth = 1;
feedback->float_length = POSE_DIMENSION;
feedback->float_length.data_length = POSE_DIMENSION;
feedback->int_data_array = {coll};
feedback->float_data_array = {pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]};
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Publishing feedback for command id: %ld, joints: %f, %f, %f, %f, %f, %f, %f",
@@ -283,12 +283,15 @@ void RmArmNode::ArmActionFinish(int64_t index, int result)
auto goal_handle = goal_handle_pair->second;
auto goalResult = std::make_shared<ArmAction::Result>();
float pose[POSE_DIMENSION] = {0};
RmArmHandler *handle = GetArmHandler(this, goal_handle->get_goal()->body_id);
RmArmHandler *handle = GetArmHandler(this, goal_handle->get_goal()->body_id.body_id);
RCLCPP_INFO(rclcpp::get_logger(GET_FUNC_LINE_STR()), "Finishing goal joints: %f, %f, %f, %f, %f, %f",
handle->nowJoints_[0], handle->nowJoints_[1], handle->nowJoints_[2],
handle->nowJoints_[3], handle->nowJoints_[4], handle->nowJoints_[5]);
if (handle != NULL && handle->armStatus != ARM_STATUS_ERROR && handle->armStatus != ARM_STATUS_INIT) {
handle->trapezoidalTrajectory_->armKinematics_->ForwardKinematics(handle->nowJoints_, pose);
}
goalResult->end_time = endColTime.nanoseconds();
goalResult->result = result;
goalResult->result.result = result;
goalResult->command_id = index;
goalResult->pose.position.x = pose[POSE_POSITION_X];
goalResult->pose.position.y = pose[POSE_POSITION_Y];
@@ -380,7 +383,7 @@ void RmArmNode::ArmActionCheck()
if (currentTimeNs >= nextCheckPoint) {
step++;
nextCheckPoint += ARM_FOLLOWING_CHECKING;
int coll = 0;//armSimulator_->CheckCollision();
int coll = armSimulator_->CheckCollision();
g_checkCnt++;
int64_t durationCol = (get_clock()->now().nanoseconds() - currentTimeNs);