HiveCoreRD/hivecore_robot_arm
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改interface模式

Browse Source
This commit is contained in:
zj
2025-10-24 19:28:02 +08:00
parent 7a351cb41e
commit 3d387b2bba
30 changed files with 2116 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
arm_keyboard_input/include/arm_keyboard_node.hpp
View File

@@ -5,7 +5,7 @@
#include "std_msgs/msg/float64_multi_array.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "interfaces/action/arm.hpp"
#include "arm_define.h"
#include "arm_action_define.h"
#include "rm_define.h"
using ArmAction = interfaces::action::Arm;

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.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.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.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.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.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE; // 位置控制
goal_msg.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.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.body_id = armId;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE; // 位置控制
goal_msg.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.action_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.data_length.data_length = POSE_DIMENSION;
goal_msg.data_type = ARM_COMMAND_TYPE_POSE_STEP_ON; // 位置控制
goal_msg.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.body_id = LEFT_ARM;
goal_msg.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.body_id = RIGHT_ARM;
goal_msg.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.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.body_id = LEFT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.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.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.body_id = RIGHT_ARM;
goal_msg.data_type = ARM_COMMAND_TYPE_ANGLE_STEP_ON; // 位置控制
goal_msg.data_length = USED_ARM_DOF;
goal_msg.command_id = commandId;
for (int i = 0; i < USED_ARM_DOF; ++i) {