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

new schedual

Browse Source
This commit is contained in:
NuoDaJia02
2025-12-29 15:14:54 +08:00
parent f56490c5b1
commit acffa0e6db
5 changed files with 205 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
src/brain/config/bt_vision_grasp.params.yaml
View File

@@ -23,6 +23,26 @@
'
- name: s1_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'
- name: s1_wheel_move_to_origin_pickup_position
params: 'move_distance: 0.5
move_angle: 0.0
'
- name: s2_right_arm_pre_cam_take_photo
params: 'body_id: 2
data_type: 2
@@ -121,14 +141,34 @@
frame_time_stamp: 0
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, -45, -22.88, -16.30, 112.10]
'
- name: s6_move_waist_turn_left_180
- name: s6_move_waist_turn_left_90
params: 'move_pitch_degree: 0.0
move_yaw_degree: 180.0
move_yaw_degree: 90.0
'
- name: s6_wheel_move_to_putdown_position
params: 'move_distance: 0.0
move_angle: 0.0
'
- name: s7_right_arm_grasp_box
params: 'body_id: 2
data_type: 2
data_length: 12
command_id: 0
frame_time_stamp: 0
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s7_right_arm_put_down_box
params: 'body_id: 2
@@ -141,7 +181,7 @@
frame_time_stamp: 0
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 83.78, -88.95, 13.97, 22.88, -16.29, 67.99]
data_array: [85, 24, 88, -175, -68, -90, 83.78, -88.95, 13.97, 22.88, -16.29, 67.99]
'
- name: s8_gripper_open
@@ -165,7 +205,7 @@
frame_time_stamp: 0
data_array: [89.83, 20.24, 95.26, -175.23, -65.09, -90.85, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
data_array: [85, 24, 88, -175, -68, -90, 96.30, -88.95, 13.98, -22.88, -16.30, 112.10]
'
- name: s10_move_waist_turn_right_90
@@ -185,6 +225,6 @@
frame_time_stamp: 0
data_array: [110.38, 24.02, 89.51, -175.36, -65.38, -70.06, 60.27, -20.97, -87.36, 5.58, -68.56, 58.21]
data_array: [85, 24, 88, -175, -68, -90, 95, -24, -88, -5, -68, 90]
'

18
src/brain/config/bt_vision_grasp.xml
View File

@@ -5,9 +5,11 @@
<Sequence>
<Arm_H name="s1_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_1">
<MoveWheel_H name="s1_wheel_move_to_origin_pickup_position" />
<!-- <MoveWaist_H name="s1_move_waist_turn_right_90" /> -->
<GripperCmd_H name="s1_gripper_open" />
</Parallel>
<Arm_H name="s2_right_arm_pre_cam_take_photo" />
<RetryUntilSuccessful name="retry_camera_vision_recg_1" num_attempts="3">
<VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
</RetryUntilSuccessful>
@@ -20,19 +22,23 @@
<Arm_H name="s3_right_arm_vision_pre_grasp" />
<Arm_H name="s4_right_arm_vision_grasp" />
<GripperCmd_H name="s4_gripper_close" />
<Arm_H name="s4_right_arm_take_box" />
<Arm_H name="s5_right_arm_grasp_box" />
<Parallel name="parallel_action_2">
<Arm_H name="s5_right_arm_grasp_box" />
<!-- <MoveWaist_H name="s6_move_waist_turn_left_180" /> -->
<MoveWaist_H name="s6_move_waist_turn_left_90" />
<MoveWheel_H name="s6_wheel_move_to_putdown_position" />
</Parallel>
<Arm_H name="s7_right_arm_grasp_box" />
<Arm_H name="s7_right_arm_put_down_box" />
<GripperCmd_H name="s8_gripper_open" />
<Arm_H name="s9_right_arm_grasp_box" />
<!-- <MoveWaist_H name="s10_move_waist_turn_right_90" /> -->
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
<Parallel name="parallel_action_3">
<MoveWaist_H name="s10_move_waist_turn_right_90" />
<Arm_H name="s11_right_arm_pre_cam_take_photo" />
</Parallel>
</Sequence>
</RetryUntilSuccessful>
</Sequence>

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

@@ -43,15 +43,20 @@ public:
*/
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
std::vector<geometry_msgs::msg::Pose> right_hand_take_photo_pose_;
std::vector<geometry_msgs::msg::Pose> right_hand_take_photo_poses_;
std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
std::vector<geometry_msgs::msg::Pose> grasp_poses_;
geometry_msgs::msg::Pose right_hand_take_object_pose_;
std::vector<geometry_msgs::msg::Pose> right_hand_take_object_pose_;
std::vector<std::vector<double>> right_hand_take_photo_angles_;
std::vector<std::vector<double>> pre_grasp_angles_;
std::vector<std::vector<double>> grasp_angles_;
std::vector<std::vector<double>> right_hand_take_object_angles_;
std::vector<double> left_arm_joint_angles_;
std::vector<double> right_arm_joint_angles_;
std::mutex joint_state_mutex_;
std::vector<double> right_arm_cam_joint_angles_;
std::vector<double> right_arm_take_photo_joint_angles_;
std::string camera_position_;

5
src/brain/include/brain/payload_converters.hpp
View File

@@ -303,7 +303,7 @@ inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo:
return r;
}
// MotorParam::Request: motor_id, max_speed, accel
// MotorParam::Request: motor_id, max_speed, add_acc, dec_acc
template<>
inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorParam::Request>(const YAML::Node & n)
{
@@ -311,7 +311,8 @@ inline interfaces::srv::MotorParam::Request from_yaml<interfaces::srv::MotorPara
if (n && n.IsMap()) {
if (n["motor_id"]) r.motor_id = n["motor_id"].as<uint16_t>();
if (n["max_speed"]) r.max_speed = n["max_speed"].as<uint16_t>();
if (n["accel"]) r.accel = n["accel"].as<uint16_t>();
if (n["add_acc"]) r.add_acc = n["add_acc"].as<uint16_t>();
if (n["dec_acc"]) r.dec_acc = n["dec_acc"].as<uint16_t>();
}
return r;
}

209
src/brain/src/cerebellum_hooks.cpp
View File

@@ -77,50 +77,54 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
goal.data_length = USED_ARM_DOF;
}
} else if (goal.data_type == 90) { // 90: Set camera pose
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
auto angle = node->right_arm_cam_joint_angles_;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
goal.data_length = USED_ARM_DOF;
} else if (goal.data_type >= 100 && goal.data_type <= 110) {
geometry_msgs::msg::Pose pose;
if (goal.data_type == 100) {
if (!node->right_hand_take_photo_pose_.empty()) {
pose = node->right_hand_take_photo_pose_[0];
std::vector<double> angle{0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
if (goal.data_type == 100) { //take photo
if (!node->right_hand_take_photo_poses_.empty()) {
pose = node->right_hand_take_photo_poses_[0];
}
}
else if (goal.data_type == 101) { // pre-grasp
if (!node->right_hand_take_photo_angles_.empty()) {
angle = node->right_hand_take_photo_angles_[0];
}
} else if (goal.data_type == 101) { // pre-grasp
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[0];
}
if (!node->pre_grasp_angles_.empty()) {
angle = node->pre_grasp_angles_[0];
}
} else if (goal.data_type == 102) { // grasp
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[0];
}
pose.position.y += 0.03;
node->right_hand_take_object_pose_ = pose;
node->right_hand_take_object_pose_.position.x -= 0.06;
} else if (goal.data_type == 103) { // left arm pre-grasp
if (!node->pre_grasp_poses_.empty()) {
pose = node->pre_grasp_poses_[1];
if (!node->grasp_angles_.empty()) {
angle = node->grasp_angles_[0];
}
} else if (goal.data_type == 104) { // left arm grasp
if (!node->grasp_poses_.empty()) {
pose = node->grasp_poses_[1];
} else if (goal.data_type == 110) { //take object
if (!node->right_hand_take_object_pose_.empty()) {
pose = node->right_hand_take_object_pose_[0];
}
if (!node->right_hand_take_object_angles_.empty()) {
angle = node->right_hand_take_object_angles_[0];
}
} else if (goal.data_type == 110) {
pose = node->right_hand_take_object_pose_;
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
return goal;
}
goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
goal.data_length = POSE_DIMENSION;
goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
// goal.data_type = ARM_COMMAND_TYPE_POSE_DIRECT_MOVE;
// goal.data_length = POSE_DIMENSION;
// goal.data_array = {pose.position.x, pose.position.y, pose.position.z,
// pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (angle.size() == USED_ARM_DOF) {
goal.data_type = ARM_COMMAND_TYPE_ANGLE_DIRECT_MOVE;
goal.data_length = USED_ARM_DOF;
goal.data_array = {angle[0], angle[1], angle[2], angle[3], angle[4], angle[5]};
} else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid angle size: %ld/%d", angle.size(), USED_ARM_DOF);
}
}
else {
RCLCPP_ERROR(node->get_logger(), "ARM make goal error: data_type[%d] not support", goal.data_type);
@@ -597,8 +601,19 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// 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);
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if ((node->arm_target_frame_.find("right") != std::string::npos)) {
current_joint_angles = node->right_arm_joint_angles_;
} else {
current_joint_angles = node->left_arm_joint_angles_;
}
}
if (node->camera_position_ == "top") {
node->right_hand_take_photo_pose_.clear();
node->right_hand_take_photo_poses_.clear();
node->right_hand_take_photo_angles_.clear();
geometry_msgs::msg::Pose pose = node->target_pose_[node->target_frame_].pose;
RCLCPP_INFO(node->get_logger(), "[%s] target pose %s frame, camera_position: %s",
@@ -608,7 +623,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pose.orientation.y, pose.orientation.z, pose.orientation.w);
pose.position.x += 0.08f;
pose.position.y -= 0.40f;
// pose.position.y -= 0.40f;
pose.position.y = 0.34f;
pose.position.z += 0.03f;
pose.orientation.x = -0.7071;
pose.orientation.y = 0.0000;
@@ -624,7 +640,22 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
return false;
}
node->right_hand_take_photo_pose_.push_back(pose);
std::vector<double> joint_angles = current_joint_angles;
tf2::Vector3 target_pos = {pose.position.x, pose.position.y, pose.position.z};
tf2::Quaternion target_quat = {pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w};
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(target_pos, target_quat, joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "right_hand take_photo_pose rm arm inverse kinematics failed.");
return false;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "right_hand take_photo angles: ");
for (const auto & angles: joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
#endif
node->right_hand_take_photo_poses_.push_back(pose);
node->right_hand_take_photo_angles_.push_back(joint_angles);
return true;
}
@@ -643,7 +674,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
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);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z - node->grab_width_[1] * 0.1f);
tf2::Vector3 target_pos(pose_in_arm.pose.position.x, pose_in_arm.pose.position.y, pose_in_arm.pose.position.z);
tf2::Quaternion target_quat(pose_in_arm.pose.orientation.x, pose_in_arm.pose.orientation.y, pose_in_arm.pose.orientation.z, pose_in_arm.pose.orientation.w);
// std::vector<double> best_angles = brain::GraspPoseCalculator::find_optimal_grasp_angles(target_pos, node->arm_target_frame_, 3);
@@ -658,39 +689,65 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
{
std::vector<std::string> palm_facings = {"down"}; //{"up", "down"};
std::vector<double> current_joint_angles;
{
std::lock_guard<std::mutex> lk(node->joint_state_mutex_);
if ((node->arm_target_frame_.find("right") != std::string::npos)) {
current_joint_angles = node->right_arm_joint_angles_;
} else {
current_joint_angles = node->left_arm_joint_angles_;
}
}
for (double angle : best_angles) {
for (const auto& palm : palm_facings) {
auto result = brain::GraspPoseCalculator::calculate(target_pos,
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, "side", angle, palm, 0.10, 0.20, node->arm_target_frame_);
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
std::vector<double> grasp_joint_angles = current_joint_angles;
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, pre_grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(),
result.pre_grasp_pose.orientation.w());
#endif
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
//pre grasp
std::vector<double> pre_grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.pre_grasp_pose.position,
result.pre_grasp_pose.orientation, pre_grasp_joint_angles) != 0) {
RCLCPP_WARN(node->get_logger(), "[%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s pre_grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
//grasp
result.grasp_pose.position.setY(result.grasp_pose.position.y() + 0.02); //补偿抓取的长度
std::vector<double> grasp_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
//take object
auto take_object_pose = result.grasp_pose;
take_object_pose.position.setX(take_object_pose.position.x() - 0.06); //向上拿起物体
std::vector<double> take_object_joint_angles = current_joint_angles;
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(take_object_pose.position,
take_object_pose.orientation, take_object_joint_angles) != 0) {
RCLCPP_ERROR(node->get_logger(), "[%s] angle %f, palm %s take_object joint_angles rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "pre_grasp_joint_angles: ");
for (const auto & angles: pre_grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
RCLCPP_INFO(node->get_logger(), "grasp_joint_angles: ");
for (const auto & angles: grasp_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
RCLCPP_INFO(node->get_logger(), "take_object_joint_angles: ");
for (const auto & angles: take_object_joint_angles) {
RCLCPP_INFO(node->get_logger(), "%.4f", angles);
}
#endif
geometry_msgs::msg::Pose pre_grasp_msg;
pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
@@ -699,21 +756,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
#ifdef DEBUG_GRASP_POSE_CALCULATION
RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]",
skill_name.c_str(), angle, palm.c_str(),
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
#endif
if (brain::GraspPoseCalculator::rm_arm_inverse_kinematics(result.grasp_pose.position,
result.grasp_pose.orientation, grasp_joint_angles) != 0) {
RCLCPP_WARN(node->get_logger(), "[%s] angle %f, palm %s grasp_pose rm arm inverse kinematics failed.",
skill_name.c_str(), angle, palm.c_str());
continue;
}
geometry_msgs::msg::Pose grasp_msg;
grasp_msg.position.x = result.grasp_pose.position.x();
grasp_msg.position.y = result.grasp_pose.position.y();
@@ -725,14 +767,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
node->pre_grasp_poses_.push_back(pre_grasp_msg);
node->grasp_poses_.push_back(grasp_msg);
node->right_hand_take_object_pose_.push_back(grasp_msg);
RCLCPP_INFO(node->get_logger(), "Generated Pose (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
RCLCPP_INFO(node->get_logger(), " Pre: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
result.pre_grasp_pose.position.x(), result.pre_grasp_pose.position.y(), result.pre_grasp_pose.position.z(),
result.pre_grasp_pose.orientation.x(), result.pre_grasp_pose.orientation.y(), result.pre_grasp_pose.orientation.z(), result.pre_grasp_pose.orientation.w());
RCLCPP_INFO(node->get_logger(), " Grasp: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f",
result.grasp_pose.position.x(), result.grasp_pose.position.y(), result.grasp_pose.position.z(),
result.grasp_pose.orientation.x(), result.grasp_pose.orientation.y(), result.grasp_pose.orientation.z(), result.grasp_pose.orientation.w());
node->pre_grasp_angles_.push_back(pre_grasp_joint_angles);
node->grasp_angles_.push_back(grasp_joint_angles);
node->right_hand_take_object_angles_.push_back(take_object_joint_angles);
RCLCPP_INFO(node->get_logger(), "Generated Pose Success (grasp_type: %s, Angle: %.1f, Palm: %s)", result.name.c_str(), angle, palm.c_str());
}
}
}
@@ -785,6 +826,11 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
node->pre_grasp_poses_.clear();
node->grasp_poses_.clear();
node->right_hand_take_object_pose_.clear();
node->pre_grasp_angles_.clear();
node->grasp_angles_.clear();
node->right_hand_take_object_angles_.clear();
for (size_t i = 0; i < resp->objects.size(); ++i) {
const auto & obj = resp->objects[i];
@@ -800,10 +846,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
}
if (node->camera_position_ == "top") {
if (node->right_hand_take_photo_pose_.empty()) {
if (node->right_hand_take_photo_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo poses generated", skill_name.c_str());
return false;
}
if (node->right_hand_take_photo_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid right hand take photo angles generated", skill_name.c_str());
return false;
}
} else {
if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
@@ -814,10 +864,23 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
return false;
}
if (node->pre_grasp_angles_.empty() || node->grasp_angles_.empty() || node->right_hand_take_object_angles_.empty()) {
RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
return false;
}
if (node->pre_grasp_angles_.size() != node->grasp_angles_.size() ||
node->grasp_angles_.size() != node->right_hand_take_object_angles_.size() ||
node->pre_grasp_angles_.size() != node->right_hand_take_object_angles_.size()) {
RCLCPP_WARN(node->get_logger(), "[%s] angles size not match [%zu %zu %zu]",
skill_name.c_str(), node->pre_grasp_angles_.size(), node->grasp_angles_.size(),
node->right_hand_take_object_angles_.size());
return false;
}
}
node->right_arm_cam_joint_angles_.clear();
node->right_arm_cam_joint_angles_ = node->right_arm_joint_angles_;
node->right_arm_take_photo_joint_angles_.clear();
node->right_arm_take_photo_joint_angles_ = node->right_arm_joint_angles_;
return true;
};