move cerebellum hooks funcs to cerebellum_hooks.hpp/cpp
This commit is contained in:
@@ -34,6 +34,7 @@ find_package(tf2_geometry_msgs REQUIRED)
|
||||
add_executable(brain_node
|
||||
src/brain_node.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/cerebellum_hooks.cpp
|
||||
src/cerebrum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
|
||||
35
src/brain/include/brain/cerebellum_hooks.hpp
Normal file
35
src/brain/include/brain/cerebellum_hooks.hpp
Normal file
@@ -0,0 +1,35 @@
|
||||
#ifndef BRAIN_CEREBELLUM_HOOKS_HPP_
|
||||
#define BRAIN_CEREBELLUM_HOOKS_HPP_
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
class CerebellumNode;
|
||||
|
||||
class CerebellumHooks
|
||||
{
|
||||
public:
|
||||
static void ConfigureActionHooks(CerebellumNode * node);
|
||||
static void ConfigureArmSpaceControlHooks(CerebellumNode * node);
|
||||
static void ConfigureArmHooks(CerebellumNode * node);
|
||||
static void ConfigureHandControlHooks(CerebellumNode * node);
|
||||
static void ConfigureCameraTakePhotoHooks(CerebellumNode * node);
|
||||
static void ConfigureLegControlHooks(CerebellumNode * node);
|
||||
static void ConfigureVisionGraspObjectHooks(CerebellumNode * node);
|
||||
static void ConfigureSlamModeHooks(CerebellumNode * node);
|
||||
static void ConfigureNavigateToPoseHooks(CerebellumNode * node);
|
||||
static void ConfigureMoveWaistHooks(CerebellumNode * node);
|
||||
static void ConfigureMoveLegHooks(CerebellumNode * node);
|
||||
static void ConfigureMoveWheelHooks(CerebellumNode * node);
|
||||
static void ConfigureMoveHomeHooks(CerebellumNode * node);
|
||||
static void ConfigureJzCmdControlHooks(CerebellumNode * node);
|
||||
|
||||
static void ConfigureServiceHooks(CerebellumNode * node);
|
||||
static void ConfigureVisionObjectRecognitionHooks(CerebellumNode * node);
|
||||
static void ConfigureMapLoadHooks(CerebellumNode * node);
|
||||
static void ConfigureMapSaveHooks(CerebellumNode * node);
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
#endif // BRAIN_CEREBELLUM_HOOKS_HPP_
|
||||
@@ -46,6 +46,7 @@ private:
|
||||
std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;
|
||||
std::mutex goal_ctxs_mutex_;
|
||||
static thread_local const std::vector<interfaces::msg::SkillCall>* tls_current_calls_;
|
||||
static thread_local int tls_arm_body_id;
|
||||
// Generic action registry to support multiple actions via registration
|
||||
std::unique_ptr<ActionServerRegistry> action_registry_;
|
||||
std::unique_ptr<ActionClientRegistry> action_clients_;
|
||||
@@ -111,6 +112,8 @@ private:
|
||||
std::atomic<double> nav2_last_distance_remaining_{std::numeric_limits<double>::quiet_NaN()};
|
||||
std::atomic<bool> nav2_last_goal_succeeded_{false};
|
||||
|
||||
friend class CerebellumHooks;
|
||||
|
||||
// ---- helpers ----
|
||||
/**
|
||||
* @brief Try to parse the current goal's per-call YAML payload into a typed object.
|
||||
@@ -140,27 +143,6 @@ private:
|
||||
}
|
||||
return false;
|
||||
}
|
||||
/** @brief Install application-specific action hook overrides for skill manager. */
|
||||
void ConfigureActionHooks();
|
||||
void ConfigureArmSpaceControlHooks();
|
||||
void ConfigureArmHooks();
|
||||
void ConfigureHandControlHooks();
|
||||
void ConfigureCameraTakePhotoHooks();
|
||||
void ConfigureLegControlHooks();
|
||||
void ConfigureVisionGraspObjectHooks();
|
||||
void ConfigureSlamModeHooks();
|
||||
void ConfigureNavigateToPoseHooks();
|
||||
void ConfigureMoveWaistHooks();
|
||||
void ConfigureMoveLegHooks();
|
||||
void ConfigureMoveWheelHooks();
|
||||
void ConfigureMoveHomeHooks();
|
||||
void ConfigureJzCmdControlHooks();
|
||||
|
||||
/** @brief Install application-specific service hook overrides. */
|
||||
void ConfigureServiceHooks();
|
||||
void ConfigureVisionObjectRecognitionHooks();
|
||||
void ConfigureMapLoadHooks();
|
||||
void ConfigureMapSaveHooks();
|
||||
|
||||
/**
|
||||
* @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
|
||||
|
||||
688
src/brain/src/cerebellum_hooks.cpp
Normal file
688
src/brain/src/cerebellum_hooks.cpp
Normal file
@@ -0,0 +1,688 @@
|
||||
#include "brain/cerebellum_hooks.hpp"
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "brain/payload_converters.hpp"
|
||||
#include "arm_action_define.h"
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
void CerebellumHooks::ConfigureActionHooks(CerebellumNode * node)
|
||||
{
|
||||
if (!node->skill_manager_) {
|
||||
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping action hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureArmHooks(node);
|
||||
ConfigureHandControlHooks(node);
|
||||
ConfigureCameraTakePhotoHooks(node);
|
||||
ConfigureMoveWaistHooks(node);
|
||||
ConfigureMoveLegHooks(node);
|
||||
ConfigureMoveWheelHooks(node);
|
||||
ConfigureMoveHomeHooks(node);
|
||||
ConfigureJzCmdControlHooks(node);
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureArmSpaceControlHooks(CerebellumNode * node)
|
||||
{
|
||||
(void)node;
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
|
||||
{
|
||||
// Arm
|
||||
SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::Arm::Goal goal{};
|
||||
if (!node->TryParseCallPayload<interfaces::action::Arm::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s]Received Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
|
||||
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
|
||||
|
||||
goal.body_id = (CerebellumNode::tls_arm_body_id == LEFT_ARM ||
|
||||
CerebellumNode::tls_arm_body_id == RIGHT_ARM) ? CerebellumNode::tls_arm_body_id : goal.body_id;
|
||||
goal.command_id = ++node->command_id_;
|
||||
if (goal.body_id == LEFT_ARM) {
|
||||
node->command_id_left_arm_ = goal.command_id;
|
||||
} else if (goal.body_id == RIGHT_ARM) {
|
||||
node->command_id_right_arm_ = goal.command_id;
|
||||
}
|
||||
goal.frame_time_stamp = node->get_clock()->now().nanoseconds();
|
||||
|
||||
if (goal.data_type == ARM_COMMAND_TYPE_POSE_DIRECT_MOVE) {
|
||||
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]};
|
||||
} else if (goal.body_id == RIGHT_ARM) {
|
||||
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;
|
||||
}
|
||||
} 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]};
|
||||
} else if (goal.body_id == RIGHT_ARM) {
|
||||
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;
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
|
||||
return goal;
|
||||
}
|
||||
|
||||
if (goal.data_array.size() >= USED_ARM_DOF) {
|
||||
RCLCPP_INFO(node->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]",
|
||||
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
|
||||
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_array size[%ld]", goal.data_array.size());
|
||||
}
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
|
||||
if (!feedback) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] feedback is empty", skill_name.c_str());
|
||||
return;
|
||||
}
|
||||
(void)feedback;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
|
||||
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == OK);
|
||||
const std::string message = res.result ? std::string("action end") : std::string("No return information");
|
||||
|
||||
geometry_msgs::msg::Pose pose = res.result->pose;
|
||||
std::string res_arm_name = "";
|
||||
if (res.result->command_id == node->command_id_left_arm_) {
|
||||
node->final_arm_pose_["left_arm"] = pose;
|
||||
res_arm_name = "Left Arm";
|
||||
} else if (res.result->command_id == node->command_id_right_arm_) {
|
||||
node->final_arm_pose_["right_arm"] = pose;
|
||||
res_arm_name = "Right Arm";
|
||||
}
|
||||
|
||||
RCLCPP_WARN(node->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(node->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(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
|
||||
{
|
||||
// HandControl
|
||||
SkillManager::ActionHookOptions<interfaces::action::HandControl> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::HandControl::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!node->TryParseCallPayload<interfaces::action::HandControl::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->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 (!node->has_parameter(param)) {
|
||||
node->declare_parameter<std::vector<double>>(param, {0.0, 0.0, 0.0});
|
||||
}
|
||||
const auto values = node->get_parameter(param).as_double_array();
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Target parameters size=%zu", skill_name.c_str(), values.size());
|
||||
(void)values;
|
||||
goal.mode = 1;
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)feedback;
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureCameraTakePhotoHooks(CerebellumNode * node)
|
||||
{
|
||||
// CameraTakePhoto
|
||||
SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::CameraTakePhoto::Goal goal{};
|
||||
if (!node->TryParseCallPayload<interfaces::action::CameraTakePhoto::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
return goal;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureLegControlHooks(CerebellumNode * node)
|
||||
{
|
||||
(void)node;
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureVisionGraspObjectHooks(CerebellumNode * node)
|
||||
{
|
||||
(void)node;
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureSlamModeHooks(CerebellumNode * node)
|
||||
{
|
||||
(void)node;
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureNavigateToPoseHooks(CerebellumNode * node)
|
||||
{
|
||||
(void)node;
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMoveWaistHooks(CerebellumNode * node)
|
||||
{
|
||||
// MoveWaist
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWaist> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::MoveWaist::Goal goal{};
|
||||
// Plan A: load per-call YAML payload into goal if present
|
||||
if (!node->TryParseCallPayload<interfaces::action::MoveWaist::Goal>(skill_name, goal)) {
|
||||
RCLCPP_INFO(node->get_logger(), "Loaded MoveWaist goal from YAML payload");
|
||||
return goal;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] move_pitch_degree: %f, move_yaw_degree: %f",
|
||||
skill_name.c_str(), goal.move_pitch_degree, goal.move_yaw_degree);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveWaist::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMoveLegHooks(CerebellumNode * node)
|
||||
{
|
||||
// MoveLeg
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::MoveLeg::Goal goal{};
|
||||
if (!node->TryParseCallPayload<interfaces::action::MoveLeg::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveLeg::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMoveWheelHooks(CerebellumNode * node)
|
||||
{
|
||||
// MoveWheel
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWheel> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::MoveWheel::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!node->TryParseCallPayload<interfaces::action::MoveWheel::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] move_distance=%f, move_angle=%f",
|
||||
skill_name.c_str(), goal.move_distance, goal.move_angle);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveWheel::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
|
||||
{
|
||||
// MoveHome
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveHome> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::MoveHome::Goal goal{};
|
||||
// Plan A: per-call YAML overrides (even though goal is empty)
|
||||
if (!node->TryParseCallPayload<interfaces::action::MoveHome::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::MoveHome::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)skill_name;
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
|
||||
{
|
||||
// JzCmd
|
||||
SkillManager::ActionHookOptions<interfaces::action::JzCmd> hooks;
|
||||
hooks.make_goal = [node](const std::string & skill_name) {
|
||||
interfaces::action::JzCmd::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!node->TryParseCallPayload<interfaces::action::JzCmd::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
// goal.loc = 30; //grab_width_; //TODO transfer
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, devid: %d, loc: %d, speed: %d, torque: %d, mode: %d",
|
||||
skill_name.c_str(), goal.devid, goal.loc, goal.speed, goal.torque, goal.mode);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [node](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::JzCmd::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] action feedback, devid: %d, loc: %d, speed: %d, torque: %d",
|
||||
skill_name.c_str(), feedback->devid, feedback->loc, feedback->speed, feedback->torque);
|
||||
|
||||
};
|
||||
hooks.on_result = [node](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>::WrappedResult & res) {
|
||||
|
||||
if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] action on_result, devid: %d, state: %s",
|
||||
skill_name.c_str(), res.result->devid, res.result->state.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
|
||||
}
|
||||
|
||||
};
|
||||
hooks.on_goal_response = [node](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
node->skill_manager_->configure_action_hooks<interfaces::action::JzCmd>("JzCmd", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
|
||||
{
|
||||
if (!node->skill_manager_) {
|
||||
RCLCPP_WARN(node->get_logger(), "SkillManager unavailable, skipping service hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureVisionObjectRecognitionHooks(node);
|
||||
ConfigureMapLoadHooks(node);
|
||||
ConfigureMapSaveHooks(node);
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * node)
|
||||
{
|
||||
using VisionSrv = interfaces::srv::VisionObjectRecognition;
|
||||
{
|
||||
SkillManager::ServiceHookOptions<VisionSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) {
|
||||
return std::chrono::seconds(2);
|
||||
};
|
||||
hooks.call_timeout = [](const std::string &) {
|
||||
return std::chrono::seconds(5);
|
||||
};
|
||||
hooks.make_request = [node](const std::string & skill_name) {
|
||||
auto req = std::make_shared<VisionSrv::Request>();
|
||||
// Prefer payload_yaml from calls
|
||||
if (!node->TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
|
||||
RCLCPP_ERROR(node->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
|
||||
return req;
|
||||
}
|
||||
// fallback to parameter
|
||||
// const std::string param = "vision_object_recognition.camera_position";
|
||||
// if (!node->has_parameter(param)) {
|
||||
// node->declare_parameter<std::string>(param, std::string("right"));
|
||||
// }
|
||||
// req->camera_position = node->get_parameter(param).as_string();
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
|
||||
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [node](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
if (!resp->success) {
|
||||
detail = resp->info.empty() ? std::string("Not success") : resp->info;
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] VisionObjectRecognition failed: %s",
|
||||
skill_name.c_str(), detail.c_str());
|
||||
return false;
|
||||
}
|
||||
detail = resp->info.empty() ? std::string("success pose") : resp->info;
|
||||
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
|
||||
skill_name.c_str(), resp->success, resp->objects.size(),
|
||||
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
|
||||
|
||||
for (size_t i = 0; i < resp->objects.size(); ++i) {
|
||||
const auto & obj = resp->objects[i];
|
||||
if (node->target_frame_ == obj.class_name) {
|
||||
node->target_pose_[node->target_frame_].header = resp->header;
|
||||
node->target_pose_[node->target_frame_].header.frame_id = "RightLink6";
|
||||
node->target_pose_[node->target_frame_].pose = obj.pose;
|
||||
node->target_pose_[node->target_frame_].pose.position.z -= 0.19;
|
||||
node->grab_width_ = obj.grab_width;
|
||||
RCLCPP_INFO(node->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", node->target_frame_.c_str(), node->grab_width_);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), " [%zu] class='%s' id=%d", i,
|
||||
obj.class_name.c_str(), obj.class_id);
|
||||
|
||||
const auto & p = obj.pose;
|
||||
RCLCPP_INFO(node->get_logger(),
|
||||
"target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
p.position.x, p.position.y, p.position.z,
|
||||
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose_in_arm;
|
||||
const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
|
||||
if (!transformed) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
|
||||
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
|
||||
return false;
|
||||
} else {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
|
||||
skill_name.c_str(), node->arm_target_frame_.c_str());
|
||||
|
||||
RCLCPP_INFO(node->get_logger(),
|
||||
"%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
pose_in_arm.pose.position.x,
|
||||
pose_in_arm.pose.position.y,
|
||||
pose_in_arm.pose.position.z,
|
||||
pose_in_arm.pose.orientation.x,
|
||||
pose_in_arm.pose.orientation.y,
|
||||
pose_in_arm.pose.orientation.z,
|
||||
pose_in_arm.pose.orientation.w);
|
||||
|
||||
geometry_msgs::msg::Pose pose_grasp;
|
||||
pose_grasp.position = pose_in_arm.pose.position;
|
||||
pose_grasp.orientation = node->final_arm_pose_["right_arm"].orientation;
|
||||
|
||||
RCLCPP_INFO(node->get_logger(), "Grasp Pose: ");
|
||||
RCLCPP_INFO(node->get_logger(),
|
||||
"%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
pose_grasp.position.x,
|
||||
pose_grasp.position.y,
|
||||
pose_grasp.position.z,
|
||||
pose_grasp.orientation.x,
|
||||
pose_grasp.orientation.y,
|
||||
pose_grasp.orientation.z,
|
||||
pose_grasp.orientation.w);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
};
|
||||
node->skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
|
||||
}
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMapLoadHooks(CerebellumNode * node)
|
||||
{
|
||||
using MapLoadSrv = interfaces::srv::MapLoad;
|
||||
SkillManager::ServiceHookOptions<MapLoadSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
|
||||
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
|
||||
hooks.make_request = [node](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapLoadSrv::Request>();
|
||||
if (!node->TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter
|
||||
if (!node->has_parameter("map_load_url")) {
|
||||
node->declare_parameter<std::string>("map_load_url", std::string(""));
|
||||
}
|
||||
req->map_url = node->get_parameter("map_load_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [node](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<MapLoadSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<MapLoadSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
// MapLoad response schema may not include a message field; rely on success only
|
||||
detail = resp->success ? std::string("ok") : std::string("failed");
|
||||
if (!resp->success) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
node->skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumHooks::ConfigureMapSaveHooks(CerebellumNode * node)
|
||||
{
|
||||
using MapSaveSrv = interfaces::srv::MapSave;
|
||||
SkillManager::ServiceHookOptions<MapSaveSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
|
||||
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
|
||||
hooks.make_request = [node](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapSaveSrv::Request>();
|
||||
if (!node->TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter group
|
||||
if (!node->has_parameter("map_save_url")) {
|
||||
node->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
|
||||
}
|
||||
req->map_url = node->get_parameter("map_save_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [node](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<MapSaveSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<MapSaveSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
// MapSave response schema may not include a message field; rely on success only
|
||||
detail = resp->success ? std::string("ok") : std::string("failed");
|
||||
if (!resp->success) {
|
||||
RCLCPP_WARN(node->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(node->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
node->skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(hooks));
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
@@ -4,6 +4,7 @@
|
||||
|
||||
// Own header first.
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "brain/cerebellum_hooks.hpp"
|
||||
|
||||
// C / C++ standard library.
|
||||
#include <chrono>
|
||||
@@ -54,7 +55,7 @@
|
||||
|
||||
namespace brain
|
||||
{
|
||||
static thread_local int tls_arm_body_id = -1;
|
||||
thread_local int CerebellumNode::tls_arm_body_id = -1;
|
||||
// 线程局部：当前ExecuteBtAction goal的calls（用于payload解析与topic解析）
|
||||
thread_local const std::vector<interfaces::msg::SkillCall>* CerebellumNode::tls_current_calls_ = nullptr;
|
||||
|
||||
@@ -92,8 +93,8 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
|
||||
DeclareAndLoadParameters();
|
||||
// Register action/service hook overrides first so SkillManager will pick them up
|
||||
ConfigureActionHooks();
|
||||
ConfigureServiceHooks();
|
||||
CerebellumHooks::ConfigureActionHooks(this);
|
||||
CerebellumHooks::ConfigureServiceHooks(this);
|
||||
// Then load skills which will register action/service clients and honor the overrides
|
||||
LoadSkillsFile();
|
||||
SetupStatsTimerAndPublisher();
|
||||
@@ -110,706 +111,15 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for arm control.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureArmHooks()
|
||||
{
|
||||
// Arm
|
||||
SkillManager::ActionHookOptions<interfaces::action::Arm> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::Arm::Goal 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 Parse Arm goal: body_id=%d, data_type=%d, data_length=%d, command_id=%ld, frame_time_stamp=%ld",
|
||||
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp);
|
||||
|
||||
goal.body_id = (tls_arm_body_id == LEFT_ARM ||
|
||||
tls_arm_body_id == RIGHT_ARM) ? tls_arm_body_id : goal.body_id;
|
||||
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) {
|
||||
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]};
|
||||
} else if (goal.body_id == RIGHT_ARM) {
|
||||
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;
|
||||
}
|
||||
} 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]};
|
||||
} else if (goal.body_id == RIGHT_ARM) {
|
||||
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;
|
||||
}
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
|
||||
return goal;
|
||||
}
|
||||
|
||||
if (goal.data_array.size() >= USED_ARM_DOF) {
|
||||
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]",
|
||||
skill_name.c_str(), goal.body_id, goal.data_type, goal.data_length, goal.command_id, goal.frame_time_stamp,
|
||||
goal.data_array[0], goal.data_array[1], goal.data_array[2], goal.data_array[3], goal.data_array[4], goal.data_array[5]);
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "ARM make goal error: invalid data_array size[%ld]", goal.data_array.size());
|
||||
}
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::Arm>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::Arm::Feedback> & feedback) {
|
||||
if (!feedback) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] feedback is empty", skill_name.c_str());
|
||||
return;
|
||||
}
|
||||
(void)feedback;
|
||||
// RCLCPP_INFO(this->get_logger(), "[%s] command_id: %ld, current progress: %d",
|
||||
// skill_name.c_str(), feedback->command_id, feedback->int_lenth);
|
||||
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::Arm>::WrappedResult & res) {
|
||||
const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == OK);
|
||||
const std::string message = res.result ? std::string("action end") : std::string("No return information");
|
||||
|
||||
geometry_msgs::msg::Pose pose = res.result->pose;
|
||||
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] %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 {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::Arm>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::Arm>("Arm", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for hand control.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureHandControlHooks()
|
||||
{
|
||||
// HandControl
|
||||
SkillManager::ActionHookOptions<interfaces::action::HandControl> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::HandControl::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
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});
|
||||
}
|
||||
const auto values = this->get_parameter(param).as_double_array();
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Target parameters size=%zu", skill_name.c_str(), values.size());
|
||||
(void)values;
|
||||
goal.mode = 1;
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::HandControl::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
(void)feedback;
|
||||
//TODO :
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Current progress: %.2f", skill_name.c_str(), feedback->progress);
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::HandControl>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::HandControl>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::HandControl>("HandControl", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for waist movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveWaistHooks()
|
||||
{
|
||||
// MoveWaist
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWaist> hooks;
|
||||
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
|
||||
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);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::SharedPtr,
|
||||
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,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWaist>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveWaist>("MoveWaist", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for leg movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveLegHooks()
|
||||
{
|
||||
// MoveLeg
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveLeg> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveLeg::Goal 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](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::SharedPtr,
|
||||
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,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveLeg>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveLeg>("MoveLeg", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for wheel movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveWheelHooks()
|
||||
{
|
||||
// MoveWheel
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveWheel> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::MoveWheel::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
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;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] move_distance=%f, move_angle=%f",
|
||||
skill_name.c_str(), goal.move_distance, goal.move_angle);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::SharedPtr,
|
||||
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,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveWheel>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveWheel>("MoveWheel", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for home movement.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureMoveHomeHooks()
|
||||
{
|
||||
// MoveHome
|
||||
SkillManager::ActionHookOptions<interfaces::action::MoveHome> hooks;
|
||||
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)
|
||||
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;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::SharedPtr,
|
||||
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,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::MoveHome>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for gripper control.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureJzCmdControlHooks()
|
||||
{
|
||||
// JzCmd
|
||||
SkillManager::ActionHookOptions<interfaces::action::JzCmd> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::JzCmd::Goal goal{};
|
||||
// Plan A: per-call YAML overrides
|
||||
if (!TryParseCallPayload<interfaces::action::JzCmd::Goal>(skill_name, goal)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
|
||||
return goal;
|
||||
}
|
||||
|
||||
// goal.loc = 30; //grab_width_; //TODO transfer
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] action make_goal params, devid: %d, loc: %d, speed: %d, torque: %d, mode: %d",
|
||||
skill_name.c_str(), goal.devid, goal.loc, goal.speed, goal.torque, goal.mode);
|
||||
|
||||
return goal;
|
||||
};
|
||||
hooks.on_feedback = [this](
|
||||
const std::string & skill_name,
|
||||
ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>::SharedPtr,
|
||||
const std::shared_ptr<const interfaces::action::JzCmd::Feedback> & feedback) {
|
||||
if (!feedback) {return;}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] action feedback, devid: %d, loc: %d, speed: %d, torque: %d",
|
||||
skill_name.c_str(), feedback->devid, feedback->loc, feedback->speed, feedback->torque);
|
||||
|
||||
};
|
||||
hooks.on_result = [this](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>::WrappedResult & res) {
|
||||
|
||||
if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] action on_result, devid: %d, state: %s",
|
||||
skill_name.c_str(), res.result->devid, res.result->state.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
|
||||
}
|
||||
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::JzCmd>("JzCmd", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for various movements.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureActionHooks()
|
||||
{
|
||||
if (!skill_manager_) {
|
||||
RCLCPP_WARN(this->get_logger(), "SkillManager unavailable, skipping action hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureArmHooks();
|
||||
ConfigureHandControlHooks();
|
||||
ConfigureCameraTakePhotoHooks();
|
||||
ConfigureMoveWaistHooks();
|
||||
ConfigureMoveLegHooks();
|
||||
ConfigureMoveWheelHooks();
|
||||
ConfigureMoveHomeHooks();
|
||||
ConfigureJzCmdControlHooks();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure action hooks for vision object recognition.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
|
||||
{
|
||||
using VisionSrv = interfaces::srv::VisionObjectRecognition;
|
||||
{
|
||||
SkillManager::ServiceHookOptions<VisionSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) {
|
||||
return std::chrono::seconds(2);
|
||||
};
|
||||
hooks.call_timeout = [](const std::string &) {
|
||||
return std::chrono::seconds(5);
|
||||
};
|
||||
hooks.make_request = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<VisionSrv::Request>();
|
||||
// Prefer payload_yaml from calls
|
||||
if (!TryParseCallPayload<VisionSrv::Request>(skill_name, *req)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] Payload Parse failed", skill_name.c_str());
|
||||
return req;
|
||||
}
|
||||
// fallback to parameter
|
||||
// const std::string param = "vision_object_recognition.camera_position";
|
||||
// if (!this->has_parameter(param)) {
|
||||
// this->declare_parameter<std::string>(param, std::string("right"));
|
||||
// }
|
||||
// req->camera_position = this->get_parameter(param).as_string();
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
|
||||
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<VisionSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<VisionSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
if (!resp->success) {
|
||||
detail = resp->info.empty() ? std::string("Not success") : resp->info;
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] VisionObjectRecognition failed: %s",
|
||||
skill_name.c_str(), detail.c_str());
|
||||
return false;
|
||||
}
|
||||
detail = resp->info.empty() ? std::string("success pose") : resp->info;
|
||||
const auto stamp_sec = rclcpp::Time(resp->header.stamp).seconds();
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
|
||||
skill_name.c_str(), resp->success, resp->objects.size(),
|
||||
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
|
||||
|
||||
for (size_t i = 0; i < resp->objects.size(); ++i) {
|
||||
const auto & obj = resp->objects[i];
|
||||
if (target_frame_ == obj.class_name) {
|
||||
target_pose_[target_frame_].header = resp->header;
|
||||
target_pose_[target_frame_].header.frame_id = "RightLink6";
|
||||
target_pose_[target_frame_].pose = obj.pose;
|
||||
target_pose_[target_frame_].pose.position.z -= 0.19;
|
||||
grab_width_ = obj.grab_width;
|
||||
RCLCPP_INFO(this->get_logger(), "target_frame_: %s, grab_width_: %f, updated target_pose", target_frame_.c_str(), grab_width_);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), " [%zu] class='%s' id=%d", i,
|
||||
obj.class_name.c_str(), obj.class_id);
|
||||
|
||||
const auto & p = obj.pose;
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"target_pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
p.position.x, p.position.y, p.position.z,
|
||||
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w);
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose_in_arm;
|
||||
const bool transformed = TransformPoseToArmFrame(target_pose_[target_frame_], pose_in_arm);
|
||||
if (!transformed) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
|
||||
skill_name.c_str(), pose_in_arm.header.frame_id.c_str());
|
||||
return false;
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
|
||||
skill_name.c_str(), arm_target_frame_.c_str());
|
||||
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
pose_in_arm.pose.position.x,
|
||||
pose_in_arm.pose.position.y,
|
||||
pose_in_arm.pose.position.z,
|
||||
pose_in_arm.pose.orientation.x,
|
||||
pose_in_arm.pose.orientation.y,
|
||||
pose_in_arm.pose.orientation.z,
|
||||
pose_in_arm.pose.orientation.w);
|
||||
|
||||
geometry_msgs::msg::Pose pose_grasp;
|
||||
pose_grasp.position = pose_in_arm.pose.position;
|
||||
pose_grasp.orientation = final_arm_pose_["right_arm"].orientation;
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Grasp Pose: ");
|
||||
RCLCPP_INFO(this->get_logger(),
|
||||
"%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
|
||||
pose_grasp.position.x,
|
||||
pose_grasp.position.y,
|
||||
pose_grasp.position.z,
|
||||
pose_grasp.orientation.x,
|
||||
pose_grasp.orientation.y,
|
||||
pose_grasp.orientation.z,
|
||||
pose_grasp.orientation.w);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
|
||||
}
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureCameraTakePhotoHooks()
|
||||
{
|
||||
// CameraTakePhoto
|
||||
SkillManager::ActionHookOptions<interfaces::action::CameraTakePhoto> hooks;
|
||||
hooks.make_goal = [this](const std::string & skill_name) {
|
||||
interfaces::action::CameraTakePhoto::Goal 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](
|
||||
const std::string & skill_name,
|
||||
const ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>::WrappedResult & res) {
|
||||
const bool success = res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success;
|
||||
const std::string message = res.result ? res.result->message : std::string("No return information");
|
||||
if (success) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] Success: %s", skill_name.c_str(), message.c_str());
|
||||
} else if (res.code == rclcpp_action::ResultCode::CANCELED) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] was cancelled", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "[%s] failed (code=%d): %s",
|
||||
skill_name.c_str(), static_cast<int>(res.code), message.c_str());
|
||||
}
|
||||
};
|
||||
hooks.on_goal_response = [this](
|
||||
const std::string & skill_name,
|
||||
const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::CameraTakePhoto>> & handle) {
|
||||
if (static_cast<bool>(handle)) {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] goal has been accepted by server", skill_name.c_str());
|
||||
} else {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] goal was rejected by server", skill_name.c_str());
|
||||
}
|
||||
};
|
||||
skill_manager_->configure_action_hooks<interfaces::action::CameraTakePhoto>("CameraTakePhoto", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureMapLoadHooks()
|
||||
{
|
||||
using MapLoadSrv = interfaces::srv::MapLoad;
|
||||
SkillManager::ServiceHookOptions<MapLoadSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
|
||||
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
|
||||
hooks.make_request = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapLoadSrv::Request>();
|
||||
if (!TryParseCallPayload<MapLoadSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter
|
||||
if (!this->has_parameter("map_load_url")) {
|
||||
this->declare_parameter<std::string>("map_load_url", std::string(""));
|
||||
}
|
||||
req->map_url = this->get_parameter("map_load_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<MapLoadSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<MapLoadSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
// MapLoad response schema may not include a message field; rely on success only
|
||||
detail = resp->success ? std::string("ok") : std::string("failed");
|
||||
if (!resp->success) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] MapLoad failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] MapLoad: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<MapLoadSrv>("MapLoad", std::move(hooks));
|
||||
}
|
||||
|
||||
void CerebellumNode::ConfigureMapSaveHooks()
|
||||
{
|
||||
using MapSaveSrv = interfaces::srv::MapSave;
|
||||
SkillManager::ServiceHookOptions<MapSaveSrv> hooks;
|
||||
hooks.wait_timeout = [](const std::string &) { return std::chrono::seconds(2); };
|
||||
hooks.call_timeout = [](const std::string &) { return std::chrono::seconds(5); };
|
||||
hooks.make_request = [this](const std::string & skill_name) {
|
||||
auto req = std::make_shared<MapSaveSrv::Request>();
|
||||
if (!TryParseCallPayload<MapSaveSrv::Request>(skill_name, *req)) { return req; }
|
||||
// Fallback to parameter group
|
||||
if (!this->has_parameter("map_save_url")) {
|
||||
this->declare_parameter<std::string>("map_save_url", std::string("/tmp/humanoid_map"));
|
||||
}
|
||||
req->map_url = this->get_parameter("map_save_url").as_string();
|
||||
return req;
|
||||
};
|
||||
hooks.on_response = [this](
|
||||
const std::string & skill_name,
|
||||
const SkillManager::ServiceHookOptions<MapSaveSrv>::ClientPtr &,
|
||||
const SkillManager::ServiceHookOptions<MapSaveSrv>::ResponsePtr & resp,
|
||||
std::string & detail) {
|
||||
// MapSave response schema may not include a message field; rely on success only
|
||||
detail = resp->success ? std::string("ok") : std::string("failed");
|
||||
if (!resp->success) {
|
||||
RCLCPP_WARN(this->get_logger(), "[%s] MapSave failed: %s", skill_name.c_str(), detail.c_str());
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "[%s] MapSave: %s", skill_name.c_str(), detail.c_str());
|
||||
}
|
||||
return resp->success;
|
||||
};
|
||||
skill_manager_->configure_service_hooks<MapSaveSrv>("MapSave", std::move(hooks));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Configure Service hooks for various movements.
|
||||
*
|
||||
*/
|
||||
void CerebellumNode::ConfigureServiceHooks()
|
||||
{
|
||||
if (!skill_manager_) {
|
||||
RCLCPP_WARN(this->get_logger(), "SkillManager unavailable, skipping service hook configuration");
|
||||
return;
|
||||
}
|
||||
ConfigureVisionObjectRecognitionHooks();
|
||||
ConfigureMapLoadHooks();
|
||||
ConfigureMapSaveHooks();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Declare all configurable ROS parameters and load their initial values.
|
||||
|
||||
Reference in New Issue
Block a user