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

optimize code

Browse Source
This commit is contained in:
david
2025-10-28 14:07:58 +08:00
parent e8ecc73b69
commit 9858019bee
2 changed files with 42 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -117,7 +117,10 @@ void CerebellumNode::ConfigureArmHooks()
SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::Arm::Goal goal{};
(void)TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
RCLCPP_INFO(this->get_logger(), "[%s]Received Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=",
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
@@ -132,7 +135,7 @@ void CerebellumNode::ConfigureArmHooks()
goal.frame_time_stamp = this->get_clock()->now().nanoseconds();
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
if (goal.data_length == 14 && goal.data_array.size() == 14) {
if (goal.data_length == POSE_DIMENSION*2 && goal.data_array.size() == POSE_DIMENSION*2) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5], goal.data_array[6]};
@@ -140,10 +143,10 @@ void CerebellumNode::ConfigureArmHooks()
goal.data_array = {goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11], goal.data_array[12], goal.data_array[13]};
}
goal.data_length = POSE_DIMENSION;
}
goal.data_length = POSE_DIMENSION;
} else {
if (goal.data_length == 12 && goal.data_array.size() == 12) {
} else if (goal.data_type == ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE) {
if (goal.data_length == USED_ARM_DOF*2 && goal.data_array.size() == USED_ARM_DOF*2) {
if (goal.body_id == LEFT_ARM) {
goal.data_array = {goal.data_array[0], goal.data_array[1], goal.data_array[2],
goal.data_array[3], goal.data_array[4], goal.data_array[5]};
@@ -151,9 +154,8 @@ void CerebellumNode::ConfigureArmHooks()
goal.data_array = {goal.data_array[6], goal.data_array[7], goal.data_array[8], goal.data_array[9],
goal.data_array[10], goal.data_array[11]};
}
goal.data_length = USED_ARM_DOF;
}
goal.data_length = USED_ARM_DOF;
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
}
RCLCPP_INFO(this->get_logger(), "[%s] Control params: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld, data_array=[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f]",
@@ -218,7 +220,10 @@ void CerebellumNode::ConfigureHandControlHooks()
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::HandControl::Goal goal{};
// Plan A: per-call YAML overrides
(void)TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
const std::string param = "hand_control.target_pose";
if (!this->has_parameter(param)) {
this->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
@@ -275,7 +280,10 @@ void CerebellumNode::ConfigureMoveWaistHooks()
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::MoveWaist::Goal goal{};
// Plan A: load per-call YAML payload into goal if present
(void)TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal)) {
RCLCPP_INFO(this->get_logger(), "Loaded MoveWaist goal from YAML payload");
return goal;
}
RCLCPP_INFO(this->get_logger(), "[%s] move_pitch_degree: %f, move_yaw_degree: %f",
skill_name.c_str(), goal.move_pitch_degree, goal.move_yaw_degree);
@@ -288,7 +296,7 @@ void CerebellumNode::ConfigureMoveWaistHooks()
const std::shared_ptr<const interfaces::action::MoveWaist::Feedback> & feedback) {
if (!feedback) {return;}
//TODO
(void)skill_name;
};
hooks.on_result = [this](
const std::string & skill_name,
@@ -326,7 +334,10 @@ void CerebellumNode::ConfigureMoveLegHooks()
SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::MoveLeg::Goal goal{};
(void)TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
hooks.on_feedback = [this](
@@ -335,7 +346,7 @@ void CerebellumNode::ConfigureMoveLegHooks()
const std::shared_ptr<const interfaces::action::MoveLeg::Feedback> & feedback) {
if (!feedback) {return;}
//TODO
(void)skill_name;
};
hooks.on_result = [this](
const std::string & skill_name,
@@ -374,7 +385,10 @@ void CerebellumNode::ConfigureMoveWheelHooks()
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::MoveWheel::Goal goal{};
// Plan A: per-call YAML overrides
(void)TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
@@ -384,7 +398,7 @@ void CerebellumNode::ConfigureMoveWheelHooks()
const std::shared_ptr<const interfaces::action::MoveWheel::Feedback> & feedback) {
if (!feedback) {return;}
//TODO
(void)skill_name;
};
hooks.on_result = [this](
const std::string & skill_name,
@@ -423,7 +437,10 @@ void CerebellumNode::ConfigureMoveHomeHooks()
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::MoveHome::Goal goal{};
// Plan A: per-call YAML overrides (even though goal is empty)
(void)TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
@@ -433,7 +450,7 @@ void CerebellumNode::ConfigureMoveHomeHooks()
const std::shared_ptr<const interfaces::action::MoveHome::Feedback> & feedback) {
if (!feedback) {return;}
//TODO
(void)skill_name;
};
hooks.on_result = [this](
const std::string & skill_name,
@@ -564,7 +581,10 @@ void CerebellumNode::ConfigureCameraTakePhotoHooks()
SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
hooks.make_goal = [this](const std::string & skill_name) {
interfaces::action::CameraTakePhoto::Goal goal{};
(void)TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal);
if (!TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
return goal;
}
return goal;
};
hooks.on_result = [this](
@@ -1211,7 +1231,7 @@ bool CerebellumNode::ExecuteBilateralArmAction(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
std::string & detail)
{
RCLCPP_INFO(this->get_logger(), "[Arm] send two goals: body_id=1 and body_id=0");
RCLCPP_INFO(this->get_logger(), "[Arm] send two goals: body_id=1 and body_id=0, index=%d, total_skills=%d", index, total_skills);
std::atomic<bool> finished1{false}, finished2{false};
bool ok1 = false, ok2 = false;
@@ -1551,7 +1571,8 @@ bool CerebellumNode::ValidateAndParseGoal(
}
std::ostringstream oss; oss << "[ExecuteBtAction] Received goal calls=[";
for (size_t i = 0; i < goal->calls.size(); ++i) {
if (i) oss << ','; oss << goal->calls[i].name;
if (i) oss << ',';
oss << goal->calls[i].name;
}
oss << "] (count=" << goal->calls.size() << ")";
RCLCPP_INFO(logger, "%s", oss.str().c_str());

2
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/introspection/smacc_state_machine_info.hpp vendored
View File

@@ -322,6 +322,7 @@ void processTransition(
statechart::transition<Ev, Dst> *, std::shared_ptr<SmaccStateInfo> & sourceState)
{
// RCLCPP_INFO_STREAM(getLogger(),"GOTCHA");
(void)sourceState;
}
template <typename Ev>
@@ -329,6 +330,7 @@ void processTransition(
statechart::custom_reaction<Ev> *, std::shared_ptr<SmaccStateInfo> & sourceState)
{
// RCLCPP_INFO_STREAM(getLogger(),"GOTCHA");
(void)sourceState;
}
//---------------------------------------------