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

add final_arm_pose_

Browse Source
This commit is contained in:
david
2025-11-20 16:30:17 +08:00
parent 9ecdb1be60
commit 3d79cf2eb8
2 changed files with 22 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/brain/include/brain/cerebellum_node.hpp
View File

@@ -86,6 +86,9 @@ private:
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::string target_frame_{"bottle"};
int64_t command_id_{0};
int64_t command_id_left_arm_ {0};
int64_t command_id_right_arm_{0};
std::unordered_map<std::string, geometry_msgs::msg::Pose> final_arm_pose_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string arm_target_frame_{"base_link_rightarm"};

22
src/brain/src/cerebellum_node.cpp
View File

@@ -131,6 +131,11 @@ void CerebellumNode::ConfigureArmHooks()
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
tls_arm_body_id == RIGHT_ARM) ? tls_arm_body_id : LEFT_ARM;
goal.command_id = ++command_id_;
if (goal.body_id == LEFT_ARM) {
command_id_left_arm_ = goal.command_id;
} else if (goal.body_id == RIGHT_ARM) {
command_id_right_arm_ = goal.command_id;
}
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
@@ -184,11 +189,22 @@ void CerebellumNode::ConfigureArmHooks()
const std::string message = res.result ? std::string("action end") : std::string("No return information");
geometry_msgs::msg::Pose pose = res.result->pose;
RCLCPP_WARN(this->get_logger(), "[%s] Final position: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
skill_name.c_str(), pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
std::string res_arm_name = "";
if (res.result->command_id == command_id_left_arm_) {
final_arm_pose_["left_arm"] = pose;
res_arm_name = "Left Arm";
} else if (res.result->command_id == command_id_right_arm_) {
final_arm_pose_["right_arm"] = pose;
res_arm_name = "Right Arm";
}
RCLCPP_WARN(this->get_logger(), "[%s] %s Final position: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f",
skill_name.c_str(), res_arm_name.c_str(), pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
if (success) {
RCLCPP_INFO(this->get_logger(), "[%s] command_id %ld completed: %s", skill_name.c_str(), res.result->command_id, message.c_str());
RCLCPP_INFO(this->get_logger(), "[%s] %s command_id %ld completed: %s",
skill_name.c_str(), res_arm_name.c_str(), res.result->command_id, message.c_str());
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
} else {