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

参数可配置

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-14 16:32:06 +08:00
parent f191008b5a
commit c929176625
5 changed files with 245 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/brain/config/brain_node_params.yaml
View File

@@ -70,5 +70,46 @@ cerebellum_node:
skill_timeouts: ""
# Arm-specific parameters
arm_target_frame: "base_link_rightarm"
arm_transform_timeout_sec: 2.0
arm_target_frame: "base_link_rightarm" # 右臂基座坐标系
arm_transform_timeout_sec: 2.0 # 坐标变换超时时间
# Grasp pose calculation parameters
top_cam_right_arm_z_threshold: 0.05 #满足右臂抓取的头部相机输出的z坐标阈值
top_cam_right_arm_x_offset: 0.10 #右臂相机视觉识别时的x坐标偏移
top_cam_right_arm_y: 0.33 #右臂相机视觉识别时的y坐标
top_cam_right_arm_orientation: [-0.7071, 0.0, 0.0, 0.7071] #右臂相机视觉识别时的四元数
top_cam_left_arm_z_threshold: -0.10 #满足左臂抓取的头部相机输出的z坐标阈值
top_cam_left_arm_x_offset: 0.10 #左臂相机视觉识别时的x坐标偏移
top_cam_left_arm_y: -0.33 #左臂相机视觉识别时的y坐标
top_cam_left_arm_orientation: [0.7071, 0.0, 0.0, 0.7071] #左臂相机视觉识别时的四元数
side_cam_right_arm_y_offset: 0.0 # 补偿右臂y轴方向的抓取偏差（前后）
side_cam_right_arm_z_offset: 0.02 # 补偿右臂z轴方向的抓取偏差（左右）
side_cam_left_arm_y_offset: 0.0 # 补偿左臂y轴方向的抓取偏差（前后）
side_cam_left_arm_z_offset: 0.01 # 补偿左臂z轴方向的抓取偏差（左右）
take_object_arm_x: -0.05 # 抓取物体时x坐标
left_camera_frame: "LeftLink6" # 左臂相机坐标系
right_camera_frame: "RightLink6" # 右臂相机坐标系
target_frames: ["apple", "bottle", "medicine_box"] # 识别物体的列表
target_frame: "medicine_box" # 默认识别物体
grasp_palm_facings: ["down"] # 抓取时手掌朝向
grasp_best_angles: [0.0] # 抓取时最佳角度
grasp_type: "side" # 抓取类型
grasp_safety_dist: 0.10 # 抓取安全距离
grasp_finger_length: 0.17 # 抓取手指长度
hand_control_default_mode: 1 # 手控制默认模式
gripper_default_loc: 30 # 夹爪默认位置
arm_cmd_type_take_photo: 100 # 拍照
arm_cmd_type_pre_grasp: 101 # 预抓取
arm_cmd_type_grasp: 102 # 抓取
arm_cmd_type_take_object: 110 # 拿取动作
arm_cmd_type_custom_min: 100 # 自定义动作最小值
arm_cmd_type_custom_max: 120 # 自定义动作最大值
gripper_default_speed: 0 # 夹爪默认速度
gripper_default_torque: 0 # 夹爪默认力矩
gripper_default_mode: 0 # 夹爪默认模式
# Service hook timeouts (seconds)
vision_object_recognition_wait_timeout_sec: 2.0 # 物体识别服务调用超时时间
vision_object_recognition_call_timeout_sec: 5.0 # 物体识别服务等待超时时间
map_load_wait_timeout_sec: 2.0 # 地图加载服务调用超时时间
map_load_call_timeout_sec: 5.0 # 地图加载服务等待超时时间

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

@@ -101,6 +101,48 @@ private:
double nav_goal_yaw_offset_{0.0};
double nav_goal_distance_tolerance_{0.5};
bool slam_enable_mapping_default_{true};
// Grasp pose calculation parameters
double top_cam_right_arm_z_threshold_{0.05};
double top_cam_right_arm_x_offset_{0.10};
double top_cam_right_arm_y_{0.33};
std::vector<double> top_cam_right_arm_orientation_{-0.7071, 0.0, 0.0, 0.7071};
double top_cam_left_arm_z_threshold_{-0.10};
double top_cam_left_arm_x_offset_{0.10};
double top_cam_left_arm_y_{-0.33};
std::vector<double> top_cam_left_arm_orientation_{0.7071, 0.0, 0.0, 0.7071};
double side_cam_right_arm_y_offset_{0.02};
double side_cam_right_arm_z_offset_{0.02};
double side_cam_left_arm_y_offset_{0.03};
double side_cam_left_arm_z_offset_{0.01};
double take_object_arm_x_{-0.05};
double vision_object_recognition_wait_timeout_sec_{2.0};
double vision_object_recognition_call_timeout_sec_{5.0};
double map_load_wait_timeout_sec_{2.0};
double map_load_call_timeout_sec_{5.0};
std::string left_camera_frame_{"LeftLink6"};
std::string right_camera_frame_{"RightLink6"};
std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
std::string target_frame_{"medicine_box"};
std::vector<std::string> grasp_palm_facings_{"down"};
std::vector<double> grasp_best_angles_{0.0};
std::string grasp_type_{"side"};
double grasp_safety_dist_{0.05};
double grasp_finger_length_{0.20};
int hand_control_default_mode_{1};
int gripper_default_loc_{30};
int gripper_default_speed_{0};
int gripper_default_torque_{0};
int gripper_default_mode_{0};
int arm_cmd_type_take_photo_{100};
int arm_cmd_type_pre_grasp_{101};
int arm_cmd_type_grasp_{102};
int arm_cmd_type_take_object_{110};
int arm_cmd_type_custom_min_{100};
int arm_cmd_type_custom_max_{120};
// Optional per-skill override timeout (skill_name -> seconds)
std::unordered_map<std::string, double> skill_timeouts_;
@@ -121,8 +163,6 @@ private:
std::mutex stats_mutex_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::vector<std::string> target_frames_{"apple", "bottle", "medicine_box"};
std::string target_frame_{"medicine_box"};
int64_t command_id_{0};
int64_t command_id_left_arm_ {0};
int64_t command_id_right_arm_{0};

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

@@ -31,7 +31,7 @@ void CerebellumHooks::ConfigureArmSpaceControlHooks(CerebellumNode * node)
(void)node;
}
#define USE_ARM_ANGLE_DIRECT_MOVE
#define USE_ARM_POSE_DIRECT_MOVE
void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
{
@@ -79,68 +79,68 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
}
goal.data_length = USED_ARM_DOF;
}
} else if (goal.data_type >= 100 && goal.data_type <= 120) {
} else if (goal.data_type >= node->arm_cmd_type_custom_min_ && goal.data_type <= node->arm_cmd_type_custom_max_) {
geometry_msgs::msg::Pose pose;
std::vector<double> angle{360.0, 360.0, 360.0, 360.0, 360.0, 360.0}; //invalid angles
if (goal.data_type == 100) { //take photo
if (goal.data_type == node->arm_cmd_type_take_photo_) { //take photo
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_photo_poses_.empty()) {
pose = node->right_arm_take_photo_poses_[0];
pose = node->right_arm_take_photo_poses_[node->right_arm_take_photo_poses_.size()-1];
}
if (!node->right_arm_take_photo_angles_.empty()) {
angle = node->right_arm_take_photo_angles_[node->right_arm_take_photo_angles_.size()-1];
}
} else {
if (!node->left_arm_take_photo_poses_.empty()) {
pose = node->left_arm_take_photo_poses_[0];
pose = node->left_arm_take_photo_poses_[node->left_arm_take_photo_poses_.size()-1];
}
if (!node->left_arm_take_photo_angles_.empty()) {
angle = node->left_arm_take_photo_angles_[node->left_arm_take_photo_angles_.size()-1];
}
}
} else if (goal.data_type == 101) { // pre-grasp
} else if (goal.data_type == node->arm_cmd_type_pre_grasp_) { // pre-grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_pre_grasp_poses_.empty()) {
pose = node->right_arm_pre_grasp_poses_[0];
pose = node->right_arm_pre_grasp_poses_[node->right_arm_pre_grasp_poses_.size()-1];
}
if (!node->right_arm_pre_grasp_angles_.empty()) {
angle = node->right_arm_pre_grasp_angles_[node->right_arm_pre_grasp_angles_.size()-1];
}
} else {
if (!node->left_arm_pre_grasp_poses_.empty()) {
pose = node->left_arm_pre_grasp_poses_[0];
pose = node->left_arm_pre_grasp_poses_[node->left_arm_pre_grasp_poses_.size()-1];
}
if (!node->left_arm_pre_grasp_angles_.empty()) {
angle = node->left_arm_pre_grasp_angles_[node->left_arm_pre_grasp_angles_.size()-1];
}
}
} else if (goal.data_type == 102) { // grasp
} else if (goal.data_type == node->arm_cmd_type_grasp_) { // grasp
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_grasp_poses_.empty()) {
pose = node->right_arm_grasp_poses_[0];
pose = node->right_arm_grasp_poses_[node->right_arm_grasp_poses_.size()-1];
}
if (!node->right_arm_grasp_angles_.empty()) {
angle = node->right_arm_grasp_angles_[node->right_arm_grasp_angles_.size()-1];
}
} else {
if (!node->left_arm_grasp_poses_.empty()) {
pose = node->left_arm_grasp_poses_[0];
pose = node->left_arm_grasp_poses_[node->left_arm_grasp_poses_.size()-1];
}
if (!node->left_arm_grasp_angles_.empty()) {
angle = node->left_arm_grasp_angles_[node->left_arm_grasp_angles_.size()-1];
}
}
} else if (goal.data_type == 110) { //take object
} else if (goal.data_type == node->arm_cmd_type_take_object_) { //take object
if (goal.body_id == RIGHT_ARM) {
if (!node->right_arm_take_object_pose_.empty()) {
pose = node->right_arm_take_object_pose_[0];
pose = node->right_arm_take_object_pose_[node->right_arm_take_object_pose_.size()-1];
}
if (!node->right_arm_take_object_angles_.empty()) {
angle = node->right_arm_take_object_angles_[node->right_arm_take_object_angles_.size()-1];
}
} else {
if (!node->left_arm_take_object_pose_.empty()) {
pose = node->left_arm_take_object_pose_[0];
pose = node->left_arm_take_object_pose_[node->left_arm_take_object_pose_.size()-1];
}
if (!node->left_arm_take_object_angles_.empty()) {
angle = node->left_arm_take_object_angles_[node->left_arm_take_object_angles_.size()-1];
@@ -250,7 +250,7 @@ void CerebellumHooks::ConfigureHandControlHooks(CerebellumNode * node)
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;
goal.mode = node->hand_control_default_mode_;
return goal;
};
hooks.on_feedback = [node](
@@ -622,9 +622,9 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
if (tf == obj.class_name) {
node->target_pose_[tf].header = header;
if (node->camera_position_ == "left") {
node->target_pose_[tf].header.frame_id = "LeftLink6";
node->target_pose_[tf].header.frame_id = node->left_camera_frame_;
} else if (node->camera_position_ == "right") {
node->target_pose_[tf].header.frame_id = "RightLink6";
node->target_pose_[tf].header.frame_id = node->right_camera_frame_;
}
node->target_pose_[tf].pose = obj.pose;
node->right_hand_grab_width_.clear();
@@ -655,16 +655,18 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
geometry_msgs::msg::PoseStamped target_pose = node->target_pose_[node->target_frame_];
target_pose.header.frame_id = "base_link_rightarm";
if (target_pose.pose.position.z >= 0.05f) { //right arm
if (target_pose.pose.position.z >= node->top_cam_right_arm_z_threshold_) { //right arm
target_arm = "right_arm";
target_pose.pose.position.x += 0.12f;
target_pose.pose.position.y = 0.33f;
target_pose.pose.orientation.x = -0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
target_pose.pose.orientation.w = 0.7071;
target_pose.pose.position.x += node->top_cam_right_arm_x_offset_;
target_pose.pose.position.y = node->top_cam_right_arm_y_;
if (node->top_cam_right_arm_orientation_.size() == 4) {
target_pose.pose.orientation.x = node->top_cam_right_arm_orientation_[0];
target_pose.pose.orientation.y = node->top_cam_right_arm_orientation_[1];
target_pose.pose.orientation.z = node->top_cam_right_arm_orientation_[2];
target_pose.pose.orientation.w = node->top_cam_right_arm_orientation_[3];
}
} else if (target_pose.pose.position.z <= -0.10f) { //left arm
} else if (target_pose.pose.position.z <= node->top_cam_left_arm_z_threshold_) { //left arm
//transfer to left arm base
target_arm = "left_arm";
geometry_msgs::msg::PoseStamped left_arm_pose;
@@ -675,12 +677,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
return false;
}
target_pose.pose = left_arm_pose.pose;
target_pose.pose.position.x += 0.12f;
target_pose.pose.position.y = -0.33f;
target_pose.pose.orientation.x = 0.7071;
target_pose.pose.orientation.y = 0.0000;
target_pose.pose.orientation.z = -0.0000;
target_pose.pose.orientation.w = 0.7071;
target_pose.pose.position.x += node->top_cam_left_arm_x_offset_;
target_pose.pose.position.y = node->top_cam_left_arm_y_;
if (node->top_cam_left_arm_orientation_.size() == 4) {
target_pose.pose.orientation.x = node->top_cam_left_arm_orientation_[0];
target_pose.pose.orientation.y = node->top_cam_left_arm_orientation_[1];
target_pose.pose.orientation.z = node->top_cam_left_arm_orientation_[2];
target_pose.pose.orientation.w = node->top_cam_left_arm_orientation_[3];
}
}
auto pose = target_pose.pose;
@@ -769,16 +773,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
// }
// best_angles.clear();
std::vector<double> best_angles;
best_angles.push_back(0);
std::vector<double> best_angles = node->grasp_best_angles_;
{
std::vector<std::string> palm_facings = {"down"}; //{"up", "down"};
std::vector<std::string> palm_facings = node->grasp_palm_facings_;
for (double angle : best_angles) {
for (const auto& palm : palm_facings) {
brain::GraspResult result = brain::GraspPoseCalculator::calculate(target_pos,
target_quat, "side", angle, palm, 0.03, 0.20, node->arm_target_frame_);
target_quat, node->grasp_type_, angle, palm, node->grasp_safety_dist_, node->grasp_finger_length_, node->arm_target_frame_);
#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]",
@@ -793,11 +796,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
#endif
if (node->camera_position_ == "right") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.02); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.02); //补偿抓取的偏差
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_right_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_right_arm_y_offset_); //补偿抓取的长度
} else if (node->camera_position_ == "left") {
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + 0.01); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + 0.01); //补偿抓取的偏差
result.grasp_pose.position.setZ(result.grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.pre_grasp_pose.position.setZ(result.pre_grasp_pose.position.z() + node->side_cam_left_arm_z_offset_); //补偿抓取的偏差
result.grasp_pose.position.setY(result.grasp_pose.position.y() + node->side_cam_left_arm_y_offset_); //补偿抓取的长度
}
//pre grasp
@@ -809,7 +814,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
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) {
@@ -820,7 +824,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
//take object
auto take_object_pose = result.grasp_pose;
// take_object_pose.position.setX(take_object_pose.position.x() - 0.10); //向上拿起物体
take_object_pose.position.setX(-0.05f); //向上拿起物体
take_object_pose.position.setX(node->take_object_arm_x_); //向上拿起物体
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) {
@@ -903,11 +907,13 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
{
SkillManager::ServiceHookOptions<VisionSrv> hooks;
hooks.wait_timeout = [](const std::string &) {
return std::chrono::seconds(2);
hooks.wait_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_wait_timeout_sec_));
};
hooks.call_timeout = [](const std::string &) {
return std::chrono::seconds(5);
hooks.call_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->vision_object_recognition_call_timeout_sec_));
};
hooks.make_request = [node](const std::string & skill_name) {
auto req = std::make_shared<VisionSrv::Request>();
@@ -1047,8 +1053,14 @@ 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.wait_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->map_load_wait_timeout_sec_));
};
hooks.call_timeout = [node](const std::string &) {
return std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(node->map_load_call_timeout_sec_));
};
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; }

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

@@ -154,6 +154,50 @@ void CerebellumNode::DeclareAndLoadParameters()
this->declare_parameter<std::string>("arm_target_frame", arm_target_frame_);
this->declare_parameter<double>("arm_transform_timeout_sec", arm_transform_timeout_sec_);
// Grasp pose calculation parameters
this->declare_parameter<double>("top_cam_right_arm_z_threshold", top_cam_right_arm_z_threshold_);
this->declare_parameter<double>("top_cam_right_arm_x_offset", top_cam_right_arm_x_offset_);
this->declare_parameter<double>("top_cam_right_arm_y", top_cam_right_arm_y_);
this->declare_parameter<std::vector<double>>("top_cam_right_arm_orientation", top_cam_right_arm_orientation_);
this->declare_parameter<double>("top_cam_left_arm_z_threshold", top_cam_left_arm_z_threshold_);
this->declare_parameter<double>("top_cam_left_arm_x_offset", top_cam_left_arm_x_offset_);
this->declare_parameter<double>("top_cam_left_arm_y", top_cam_left_arm_y_);
this->declare_parameter<std::vector<double>>("top_cam_left_arm_orientation", top_cam_left_arm_orientation_);
this->declare_parameter<double>("side_cam_right_arm_y_offset", side_cam_right_arm_y_offset_);
this->declare_parameter<double>("side_cam_right_arm_z_offset", side_cam_right_arm_z_offset_);
this->declare_parameter<double>("side_cam_left_arm_y_offset", side_cam_left_arm_y_offset_);
this->declare_parameter<double>("side_cam_left_arm_z_offset", side_cam_left_arm_z_offset_);
this->declare_parameter<double>("take_object_arm_x", take_object_arm_x_);
// Service hook timeouts
this->declare_parameter<double>("vision_object_recognition_wait_timeout_sec", vision_object_recognition_wait_timeout_sec_);
this->declare_parameter<double>("vision_object_recognition_call_timeout_sec", vision_object_recognition_call_timeout_sec_);
this->declare_parameter<double>("map_load_wait_timeout_sec", map_load_wait_timeout_sec_);
this->declare_parameter<double>("map_load_call_timeout_sec", map_load_call_timeout_sec_);
this->declare_parameter<std::string>("left_camera_frame", left_camera_frame_);
this->declare_parameter<std::string>("right_camera_frame", right_camera_frame_);
this->declare_parameter<std::vector<std::string>>("target_frames", target_frames_);
this->declare_parameter<std::string>("target_frame", target_frame_);
this->declare_parameter<std::vector<std::string>>("grasp_palm_facings", grasp_palm_facings_);
this->declare_parameter<std::vector<double>>("grasp_best_angles", grasp_best_angles_);
this->declare_parameter<std::string>("grasp_type", grasp_type_);
this->declare_parameter<double>("grasp_safety_dist", grasp_safety_dist_);
this->declare_parameter<double>("grasp_finger_length", grasp_finger_length_);
this->declare_parameter<int>("hand_control_default_mode", hand_control_default_mode_);
this->declare_parameter<int>("gripper_default_loc", gripper_default_loc_);
this->declare_parameter<int>("gripper_default_speed", gripper_default_speed_);
this->declare_parameter<int>("gripper_default_torque", gripper_default_torque_);
this->declare_parameter<int>("gripper_default_mode", gripper_default_mode_);
this->declare_parameter<int>("arm_cmd_type_take_photo", arm_cmd_type_take_photo_);
this->declare_parameter<int>("arm_cmd_type_pre_grasp", arm_cmd_type_pre_grasp_);
this->declare_parameter<int>("arm_cmd_type_grasp", arm_cmd_type_grasp_);
this->declare_parameter<int>("arm_cmd_type_take_object", arm_cmd_type_take_object_);
this->declare_parameter<int>("arm_cmd_type_custom_min", arm_cmd_type_custom_min_);
this->declare_parameter<int>("arm_cmd_type_custom_max", arm_cmd_type_custom_max_);
// Read back
this->get_parameter("robot_config_path", robot_config_path_);
this->get_parameter("robot_skill_file_path", robot_skill_file_path_);
@@ -172,6 +216,48 @@ void CerebellumNode::DeclareAndLoadParameters()
this->get_parameter("slam_enable_mapping_default", slam_enable_mapping_default_);
this->get_parameter("arm_target_frame", arm_target_frame_);
this->get_parameter("arm_transform_timeout_sec", arm_transform_timeout_sec_);
this->get_parameter("top_cam_right_arm_z_threshold", top_cam_right_arm_z_threshold_);
this->get_parameter("top_cam_right_arm_x_offset", top_cam_right_arm_x_offset_);
this->get_parameter("top_cam_right_arm_y", top_cam_right_arm_y_);
this->get_parameter("top_cam_right_arm_orientation", top_cam_right_arm_orientation_);
this->get_parameter("top_cam_left_arm_z_threshold", top_cam_left_arm_z_threshold_);
this->get_parameter("top_cam_left_arm_x_offset", top_cam_left_arm_x_offset_);
this->get_parameter("top_cam_left_arm_y", top_cam_left_arm_y_);
this->get_parameter("top_cam_left_arm_orientation", top_cam_left_arm_orientation_);
this->get_parameter("side_cam_right_arm_y_offset", side_cam_right_arm_y_offset_);
this->get_parameter("side_cam_right_arm_z_offset", side_cam_right_arm_z_offset_);
this->get_parameter("side_cam_left_arm_y_offset", side_cam_left_arm_y_offset_);
this->get_parameter("side_cam_left_arm_z_offset", side_cam_left_arm_z_offset_);
this->get_parameter("take_object_arm_x", take_object_arm_x_);
this->get_parameter("vision_object_recognition_wait_timeout_sec", vision_object_recognition_wait_timeout_sec_);
this->get_parameter("vision_object_recognition_call_timeout_sec", vision_object_recognition_call_timeout_sec_);
this->get_parameter("map_load_wait_timeout_sec", map_load_wait_timeout_sec_);
this->get_parameter("map_load_call_timeout_sec", map_load_call_timeout_sec_);
this->get_parameter("left_camera_frame", left_camera_frame_);
this->get_parameter("right_camera_frame", right_camera_frame_);
this->get_parameter("target_frames", target_frames_);
this->get_parameter("target_frame", target_frame_);
this->get_parameter("grasp_palm_facings", grasp_palm_facings_);
this->get_parameter("grasp_best_angles", grasp_best_angles_);
this->get_parameter("grasp_type", grasp_type_);
this->get_parameter("grasp_safety_dist", grasp_safety_dist_);
this->get_parameter("grasp_finger_length", grasp_finger_length_);
this->get_parameter("hand_control_default_mode", hand_control_default_mode_);
this->get_parameter("gripper_default_loc", gripper_default_loc_);
this->get_parameter("gripper_default_speed", gripper_default_speed_);
this->get_parameter("gripper_default_torque", gripper_default_torque_);
this->get_parameter("gripper_default_mode", gripper_default_mode_);
this->get_parameter("arm_cmd_type_take_photo", arm_cmd_type_take_photo_);
this->get_parameter("arm_cmd_type_pre_grasp", arm_cmd_type_pre_grasp_);
this->get_parameter("arm_cmd_type_grasp", arm_cmd_type_grasp_);
this->get_parameter("arm_cmd_type_take_object", arm_cmd_type_take_object_);
this->get_parameter("arm_cmd_type_custom_min", arm_cmd_type_custom_min_);
this->get_parameter("arm_cmd_type_custom_max", arm_cmd_type_custom_max_);
arm_transform_timeout_sec_ = std::max(0.01, arm_transform_timeout_sec_);
map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);

19
src/scripts/run.sh
View File

@@ -63,11 +63,20 @@ while [[ $# -gt 0 ]]; do
esac
done
timestamp() { date +"%Y%m%d_%H%M%S"; }
# 获取当前日期用于归档 (例如: 20260114)
date_day() { date +"%Y%m%d"; }
# 获取可读的时间戳用于文件名 (例如: 2026-01-14_15-30-05)
readable_timestamp() { date +"%Y-%m-%d_%H-%M-%S"; }
LOG_DIR="${WS_ROOT}/log/run/$(timestamp)"
# 1. 统一存放路径到当天的目录下
DAILY_LOG_ROOT="${WS_ROOT}/logs/$(date_day)"
# 2. 为每次启动创建一个带时间的子目录 (保证多次运行不冲突)
LOG_DIR="${DAILY_LOG_ROOT}/robot_logs_$(readable_timestamp)"
mkdir -p "${LOG_DIR}"
# 强制 ROS 2 自身的内部日志也记录到此目录，防止散落在 ~/.ros/log
export ROS_LOG_DIR="${LOG_DIR}"
pids=()
names=()
@@ -75,12 +84,14 @@ start_node() {
local name="$1"; shift
local pkg="$1"; shift
local exe="$1"; shift
local log_file="${LOG_DIR}/${name}.log"
# 3. 日志文件名可配置：在此处修改格式
local node_ts=$(readable_timestamp)
local log_file="${LOG_DIR}/${name}_${node_ts}.log"
if (( LOG_TO_FILES )); then
echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} (logs: ${log_file})"
(
# Disable ANSI colors in log files for readability
# 禁用 ANSI 颜色以保持日志文件整洁
RCUTILS_COLORIZED_OUTPUT=0 exec ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}"
) >>"${log_file}" 2>&1 &
else