diff --git a/src/brain/config/bt_carry_boxes.params.yaml b/src/brain/config/bt_carry_boxes.params.yaml index b824219..57b2f97 100644 --- a/src/brain/config/bt_carry_boxes.params.yaml +++ b/src/brain/config/bt_carry_boxes.params.yaml @@ -23,7 +23,7 @@ frame_time_stamp: 0 - data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123] + data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123] ' - name: s3_hand_pickup @@ -43,7 +43,7 @@ frame_time_stamp: 0 - data_array: [0.198632, 0.513333, 0.072526, -0.71159701, 0.13862565, -0.14251799, 0.67387035, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817] + data_array: [-0.198632, -0.513333, 0.072526, -0.13862565, -0.71159701, 0.67387035, 0.14251799, 0.253501, 0.476683, 0.265433, -0.51013004, -0.02328561, -0.48576245, 0.70940817] ' - name: s5_waist_bend_up @@ -85,7 +85,7 @@ frame_time_stamp: 0 - data_array: [0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241, 0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123] + data_array: [-0.222853, -0.514124, 0.261742, -0.18186004, -0.63142093, 0.74353241, 0.12407289, 0.325695, 0.447487, 0.108462, -0.65995416, 0.33866696, -0.16590247, 0.64980117] ' - name: s11_hand_release @@ -105,7 +105,7 @@ frame_time_stamp: 0 - data_array: [0.086304, 0.471622, 0.033076, -0.76833478, -0.08777985, -0.21307887, 0.59712123, 0.222853, 0.514124, 0.261742, -0.63142093, 0.18186004, -0.12407289, 0.74353241] + data_array: [-0.179387, -0.341708, -0.099286, -0.12851699, -0.88140507, 0.41951188, 0.17498077, 0.128250, 0.554955, 0.096719, -0.66087188, 0.14284399, -0.04440424, 0.73544015] ' - name: s13_camera_take_photo @@ -131,6 +131,6 @@ frame_time_stamp: 0 - data_array: [0.123562, 0.555266, 0.062529, -0.75662638, 0.02276690, -0.14333775, 0.63753626, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394] + data_array: [-0.123562, -0.555266, 0.062529, -0.02276690, -0.75662638, 0.63753626, 0.14333775, 0.082317, 0.557912, 0.123469, -0.63065177, -0.03018818, -0.18611339, 0.75281394] ' diff --git a/src/brain/config/bt_carry_boxes.xml b/src/brain/config/bt_carry_boxes.xml index 48b629b..3fb9aad 100644 --- a/src/brain/config/bt_carry_boxes.xml +++ b/src/brain/config/bt_carry_boxes.xml @@ -5,7 +5,7 @@ - + diff --git a/src/brain/include/brain/cerebellum_node.hpp b/src/brain/include/brain/cerebellum_node.hpp index 081a8db..540acd2 100644 --- a/src/brain/include/brain/cerebellum_node.hpp +++ b/src/brain/include/brain/cerebellum_node.hpp @@ -76,7 +76,7 @@ private: size_t total_sequence_success_ {0}; size_t total_sequence_failure_ {0}; rclcpp::TimerBase::SharedPtr stats_timer_; - double stats_log_interval_sec_ {60.0}; + double stats_log_interval_sec_ {30.0}; std::mutex stats_mutex_; rclcpp::Publisher::SharedPtr stats_pub_; std::unordered_map target_pose_; diff --git a/src/brain/src/cerebellum_node.cpp b/src/brain/src/cerebellum_node.cpp index 05c7f95..5bf271e 100644 --- a/src/brain/src/cerebellum_node.cpp +++ b/src/brain/src/cerebellum_node.cpp @@ -166,6 +166,11 @@ void CerebellumNode::ConfigureArmHooks() const ActionClientRegistry::GoalHandle::WrappedResult & res) { const bool success = (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->result == 0); const std::string message = res.result ? std::string("action end") : std::string("无返回信息"); + + geometry_msgs::msg::Pose pose = res.result->pose; + RCLCPP_WARN(this->get_logger(), "[%s] 最终位置: %.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f", + skill_name.c_str(), pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + if (success) { RCLCPP_INFO(this->get_logger(), "[%s] command_id %ld 完成: %s", skill_name.c_str(), res.result->command_id, message.c_str()); } else if (res.code == rclcpp_action::ResultCode::CANCELED) { @@ -813,12 +818,12 @@ void CerebellumNode::SetupExecuteBtServer() goal->epoch, goal->action_name.c_str(), goal->calls.size()); for (size_t i = 0; i < goal->calls.size(); ++i) { const auto & c = goal->calls[i]; - RCLCPP_INFO(this->get_logger(), " call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu", + RCLCPP_INFO(this->get_logger(), "call[%zu]: name=%s interface=%s kind=%s topic=%s payload_len=%zu", i, c.name.c_str(), c.interface_type.c_str(), c.call_kind.c_str(), c.topic.c_str(), c.payload_yaml.size()); if (!c.payload_yaml.empty()) { // Log payload content (trim if very long) const std::string payload = c.payload_yaml.size() > 512 ? c.payload_yaml.substr(0, 512) + "..." : c.payload_yaml; - RCLCPP_INFO(this->get_logger(), " payload: %s", payload.c_str()); + RCLCPP_INFO(this->get_logger(), "payload:{%s}", payload.c_str()); } } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; @@ -1145,8 +1150,8 @@ bool CerebellumNode::ExecuteActionSkill( std::this_thread::sleep_for(std::chrono::milliseconds(200)); } - if (t1.joinable()) t1.join(); - if (t2.joinable()) t2.join(); + if (t1.joinable()) t1.join(); //RIGHT ARM + if (t2.joinable()) t2.join(); //LEFT ARM if (!ok1 || !ok2) { std::ostringstream oss; diff --git a/src/brain/src/cerebrum_node.cpp b/src/brain/src/cerebrum_node.cpp index 10a199d..571b1f2 100644 --- a/src/brain/src/cerebrum_node.cpp +++ b/src/brain/src/cerebrum_node.cpp @@ -108,7 +108,7 @@ CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options) RegisterActionClient(); bt_timer_ = this->create_wall_timer(10ms, [this]() {ExecuteBehaviorTree();}); - task_timer_ = this->create_wall_timer(30000ms, [this]() {CerebrumTask();}); + task_timer_ = this->create_wall_timer(20000ms, [this]() {CerebrumTask();}); CreateServices(); diff --git a/src/scripts/rotate_pose.py b/src/scripts/rotate_pose.py new file mode 100644 index 0000000..4f09bcf --- /dev/null +++ b/src/scripts/rotate_pose.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +""" +rotate_pose.py + +Small utility to rotate a 7-element pose (x,y,z,qx,qy,qz,qw or qw,qx,qy,qz) +around a given axis by a specified angle (degrees). + +Usage examples: + ./rotate_pose.py --pose 0.1 0.2 0.3 -0.6 0.1 -0.1 0.7 --axis z --angle 180 --order qxqyqzqw --frame world + python3 rotate_pose.py --pose 0.179387 0.341708 -0.099286 -0.88140507 0.12851699 -0.17498077 0.41951188 --axis z --angle 180 --order qxqyqzqw --frame world + +Supported frames: + - world: apply rotation in world frame (q' = q_rot * q) + - body: apply rotation in body/local frame (q' = q * q_rot) + +Supported quaternion order strings: "qxqyqzqw" (default) or "wxyz" (qw,qx,qy,qz) + +""" +import argparse +import math +from typing import List, Tuple + + +def normalize_quat(q: Tuple[float, float, float, float]) -> Tuple[float, float, float, float]: + x, y, z, w = q + n = math.sqrt(x * x + y * y + z * z + w * w) + if n == 0: + return (0.0, 0.0, 0.0, 1.0) + return (x / n, y / n, z / n, w / n) + + +def axis_angle_to_quat(axis: str, angle_deg: float) -> Tuple[float, float, float, float]: + a = math.radians(angle_deg) + ca = math.cos(a / 2.0) + sa = math.sin(a / 2.0) + ax = axis.lower() + if ax == 'x': + return (sa, 0.0, 0.0, ca) + if ax == 'y': + return (0.0, sa, 0.0, ca) + if ax == 'z': + return (0.0, 0.0, sa, ca) + raise ValueError('axis must be x,y or z') + + +def quat_mul(a: Tuple[float,float,float,float], b: Tuple[float,float,float,float]) -> Tuple[float,float,float,float]: + ax, ay, az, aw = a + bx, by, bz, bw = b + x = aw*bx + ax*bw + ay*bz - az*by + y = aw*by - ax*bz + ay*bw + az*bx + z = aw*bz + ax*by - ay*bx + az*bw + w = aw*bw - ax*bx - ay*by - az*bz + return (x, y, z, w) + + +def rotate_position(pos: Tuple[float,float,float], axis: str, angle_deg: float) -> Tuple[float,float,float]: + # For 180-degree rotations we can do simple sign flips but implement general rotation using quaternion + qrot = axis_angle_to_quat(axis, angle_deg) + # convert pos to quaternion (v,0) + px, py, pz = pos + p = (px, py, pz, 0.0) + qrot_n = normalize_quat(qrot) + qrot_conj = (-qrot_n[0], -qrot_n[1], -qrot_n[2], qrot_n[3]) + tmp = quat_mul(qrot_n, p) + p_rot = quat_mul(tmp, qrot_conj) + return (p_rot[0], p_rot[1], p_rot[2]) + + +def parse_pose(vals: List[float], order: str) -> Tuple[Tuple[float,float,float], Tuple[float,float,float,float]]: + if order == 'qxqyqzqw': + if len(vals) != 7: + raise ValueError('need 7 values for pose') + return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6]) + if order == 'wxyz': + if len(vals) != 7: + raise ValueError('need 7 values for pose') + return (vals[0], vals[1], vals[2]), (vals[3], vals[4], vals[5], vals[6]) if False else (vals[4], vals[5], vals[6], vals[3]) + raise ValueError('unsupported order') + + +def format_pose(pos: Tuple[float,float,float], quat: Tuple[float,float,float,float], order: str) -> List[float]: + if order == 'qxqyqzqw': + return [pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]] + if order == 'wxyz': + # qw,qx,qy,qz + return [pos[0], pos[1], pos[2], quat[3], quat[0], quat[1], quat[2]] + raise ValueError('unsupported order') + + +def main(): + p = argparse.ArgumentParser() + p.add_argument('--pose', nargs=7, type=float, required=True, help='pose values') + p.add_argument('--axis', choices=['x','y','z'], default='z') + p.add_argument('--angle', type=float, default=180.0) + p.add_argument('--order', choices=['qxqyqzqw','wxyz'], default='qxqyqzqw') + p.add_argument('--frame', choices=['world','body'], default='world') + args = p.parse_args() + + pos, quat = parse_pose(list(args.pose), args.order) + # rotation quaternion + qrot = axis_angle_to_quat(args.axis, args.angle) + qrot = normalize_quat(qrot) + + # apply rotation to position in world frame: pos' = R * pos + pos_rot = rotate_position(pos, args.axis, args.angle) + + # apply rotation to orientation + if args.frame == 'world': + qnew = quat_mul(qrot, quat) + else: + qnew = quat_mul(quat, qrot) + qnew = normalize_quat(qnew) + + out = format_pose(pos_rot, qnew, args.order) + # Print as numeric list without quotes + print('rotated_pose:', [float('{:.8f}'.format(x)) for x in out]) + +if __name__ == '__main__': + main()