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

add JzCmd gripper support

Browse Source
This commit is contained in:
david
2025-11-21 18:22:28 +08:00
parent 9ecdb1be60
commit 53e62e2714
10 changed files with 350 additions and 147 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
src/brain/config/bt_carry_boxes_sch1.params.yaml
View File

@@ -11,7 +11,7 @@
'
- name: s1_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -25,7 +25,7 @@
'
- name: s2_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -45,7 +45,7 @@
'
- name: s3_snapshot_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -59,7 +59,7 @@
'
- name: s3_snapshot_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -73,7 +73,7 @@
'
- name: s3_snapshot_top_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -87,7 +87,7 @@
'
- name: s3_snapshot_top_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -101,7 +101,7 @@
'
- name: s3_snapshot_top_action3
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -115,7 +115,7 @@
'
- name: s4_arm_prepare_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -129,7 +129,7 @@
'
- name: s4_arm_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -143,7 +143,7 @@
'
- name: s5_arm_pickup_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -175,7 +175,7 @@
'
- name: s9_arm_put_down_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -189,7 +189,7 @@
'
- name: s10_arm_release
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -203,7 +203,7 @@
'
- name: s10_arm_raise_up
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -229,7 +229,7 @@
'
- name: s13_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -243,7 +243,7 @@
'
- name: s14_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -257,7 +257,7 @@
'
- name: s14_arm_move_pre_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -271,7 +271,7 @@
'
- name: s14_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -291,7 +291,7 @@
'
- name: s15_snapshot_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -305,7 +305,7 @@
'
- name: s15_snapshot_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -319,7 +319,7 @@
'
- name: s15_snapshot_top_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -333,7 +333,7 @@
'
- name: s15_snapshot_top_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -347,7 +347,7 @@
'
- name: s15_snapshot_top_action3
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -361,7 +361,7 @@
'
- name: s16_arm_prepare_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -375,7 +375,7 @@
'
- name: s16_arm_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -389,7 +389,7 @@
'
- name: s17_arm_pickup_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -421,7 +421,7 @@
'
- name: s21_arm_put_down_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -435,7 +435,7 @@
'
- name: s22_arm_release
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -449,7 +449,7 @@
'
- name: s22_arm_raise_up
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -475,7 +475,7 @@
'
- name: s25_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -495,7 +495,7 @@
'
- name: s26_arm_move_pre_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -509,7 +509,7 @@
'
- name: s26_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2

64
src/brain/config/bt_carry_boxes_sch2.params.yaml
View File

@@ -7,7 +7,7 @@
'
- name: s1_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -39,7 +39,7 @@
'
- name: s4_arm_move_pre_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -53,7 +53,7 @@
'
- name: s4_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -67,7 +67,7 @@
'
- name: s4_snapshot_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -81,7 +81,7 @@
'
- name: s4_snapshot_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -95,7 +95,7 @@
'
- name: s4_snapshot_top_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -109,7 +109,7 @@
'
- name: s4_snapshot_top_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -123,7 +123,7 @@
'
- name: s4_snapshot_top_action3
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -137,7 +137,7 @@
'
- name: s5_arm_prepare_to_grasp
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -151,7 +151,7 @@
'
- name: s6_arm_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -165,7 +165,7 @@
'
- name: s7_arm_pickup_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -197,7 +197,7 @@
'
- name: s11_arm_putdown_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -211,7 +211,7 @@
'
- name: s12_arm_release_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -225,7 +225,7 @@
'
- name: s12_arm_raise_up
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -239,7 +239,7 @@
'
- name: s13_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -253,7 +253,7 @@
'
- name: s13_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -291,7 +291,7 @@
'
- name: s15_arm_move_pre_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -305,7 +305,7 @@
'
- name: s15_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -319,7 +319,7 @@
'
- name: s15_snapshot_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -333,7 +333,7 @@
'
- name: s15_snapshot_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -347,7 +347,7 @@
'
- name: s15_snapshot_top_action1
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -361,7 +361,7 @@
'
- name: s15_snapshot_top_action2
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -375,7 +375,7 @@
'
- name: s15_snapshot_top_action3
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -389,7 +389,7 @@
'
- name: s16_arm_prepare_to_grasp
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -403,7 +403,7 @@
'
- name: s17_arm_grasp_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -417,7 +417,7 @@
'
- name: s18_arm_pickup_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -449,7 +449,7 @@
'
- name: s22_arm_putdown_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -463,7 +463,7 @@
'
- name: s23_arm_release_box
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -477,7 +477,7 @@
'
- name: s23_arm_raise_up
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -491,7 +491,7 @@
'
- name: s23_arm_move_to_snapshot_pose
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -505,7 +505,7 @@
'
- name: s23_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2
@@ -525,7 +525,7 @@
'
- name: s25_arm_move_origin_position
params: 'body_id: 0
params: 'body_id: 2
data_type: 2

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

@@ -7,11 +7,11 @@
'
- name: VisionObjectRecognition_H
params: 'camera_position: right
params: 'camera_position: ''''
'
- name: right_arm_vision_grasp
params: 'body_id: 0
params: 'body_id: 1
data_type: 0
@@ -24,9 +24,15 @@
data_array: []
'
- name: HandControl_H
params: 'mode: 0
- name: JzCmd_H
params: 'devid: 0
effort: 0.0
loc: 0
speed: 0
torque: 0
mode: 0
'

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

@@ -1,7 +1,7 @@
<root BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root">
<!-- Retry vision + hand until HandControl_H succeeds -->
<!-- Retry vision + hand until GripperControl_H succeeds -->
<RetryUntilSuccessful name="retry_vision_hand" num_attempts="5">
<Sequence>
<VisionObjectRecognition_H name="VisionObjectRecognition_H" />
@@ -9,7 +9,7 @@
</Sequence>
</RetryUntilSuccessful>
<!-- After Arm_H eventually succeeds, run the hand -->
<HandControl_H name="HandControl_H" />
<JzCmd_H name="JzCmd_H" />
</Sequence>
</BehaviorTree>
</root>

8
src/brain/config/robot_skills.yaml
View File

@@ -61,4 +61,10 @@
description: "回到原点位姿"
interfaces:
- name: MoveHome.action
default_topic: "MoveHome"
default_topic: "MoveHome"
- name: JzCmd
version: 1.0.0
description: "夹爪控制"
interfaces:
- JzCmd.action

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

@@ -86,6 +86,9 @@ private:
std::unordered_map<std::string, geometry_msgs::msg::PoseStamped> target_pose_;
std::string target_frame_{"bottle"};
int64_t command_id_{0};
int64_t command_id_left_arm_ {0};
int64_t command_id_right_arm_{0};
std::unordered_map<std::string, geometry_msgs::msg::Pose> final_arm_pose_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::string arm_target_frame_{"base_link_rightarm"};
@@ -151,6 +154,7 @@ private:
void ConfigureMoveLegHooks();
void ConfigureMoveWheelHooks();
void ConfigureMoveHomeHooks();
void ConfigureJzCmdControlHooks();
/** @brief Install application-specific service hook overrides. */
void ConfigureServiceHooks();

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

@@ -9,7 +9,9 @@
#include "interfaces/action/arm.hpp"
#include "interfaces/action/arm_space_control.hpp"
#include "interfaces/action/camera_take_photo.hpp"
#include "interfaces/action/execute_bt_action.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/jz_cmd.hpp"
#include "interfaces/action/leg_control.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
@@ -18,8 +20,12 @@
#include "interfaces/action/move_wheel.hpp"
#include "interfaces/action/slam_mode.hpp"
#include "interfaces/action/vision_grasp_object.hpp"
#include "interfaces/srv/bt_rebuild.hpp"
#include "interfaces/srv/jz_cmd.hpp"
#include "interfaces/srv/map_load.hpp"
#include "interfaces/srv/map_save.hpp"
#include "interfaces/srv/motor_info.hpp"
#include "interfaces/srv/motor_param.hpp"
#include "interfaces/srv/vision_object_recognition.hpp"
@@ -153,6 +159,71 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
return g;
}
// JzCmd::Goal: devid, loc, speed, torque, mode
template<>
inline interfaces::action::JzCmd::Goal from_yaml<interfaces::action::JzCmd::Goal>(const YAML::Node & n)
{
interfaces::action::JzCmd::Goal g;
if (n && n.IsMap()) {
if (n["devid"]) g.devid = n["devid"].as<uint8_t>();
if (n["loc"]) g.loc = n["loc"].as<uint8_t>();
if (n["speed"]) g.speed = n["speed"].as<uint8_t>();
if (n["torque"]) g.torque = n["torque"].as<uint8_t>();
if (n["mode"]) g.mode = n["mode"].as<uint8_t>();
}
return g;
}
// ExecuteBtAction::Goal: epoch, action_name, calls
template<>
inline interfaces::action::ExecuteBtAction::Goal from_yaml<interfaces::action::ExecuteBtAction::Goal>(const YAML::Node & n)
{
interfaces::action::ExecuteBtAction::Goal g;
if (n && n.IsMap()) {
if (n["epoch"]) g.epoch = n["epoch"].as<uint32_t>();
if (n["action_name"]) g.action_name = n["action_name"].as<std::string>();
// TODO: parse field `calls` of ROS type `interfaces/SkillCall[]` into g.calls (complex type)
}
return g;
}
// LegControl::Goal: target
template<>
inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
{
interfaces::action::LegControl::Goal g;
if (n && n.IsMap()) {
// geometry_msgs::msg::TwistStamped
if (n) {
if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
if (auto lin = n["linear"]) {
if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
}
if (auto ang = n["angular"]) {
if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
}
}
}
return g;
}
// VisionGraspObject::Goal: object_id
template<>
inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
{
interfaces::action::VisionGraspObject::Goal g;
if (n && n.IsMap()) {
if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
}
return g;
}
// MoveToPosition::Goal: target_x, target_y
template<>
inline interfaces::action::MoveToPosition::Goal from_yaml<interfaces::action::MoveToPosition::Goal>(const YAML::Node & n)
@@ -203,41 +274,61 @@ inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::A
return g;
}
// LegControl::Goal: target
// JzCmd::Request: devid, loc, speed, torque
template<>
inline interfaces::action::LegControl::Goal from_yaml<interfaces::action::LegControl::Goal>(const YAML::Node & n)
inline interfaces::srv::JzCmd::Request from_yaml<interfaces::srv::JzCmd::Request>(const YAML::Node & n)
{
interfaces::action::LegControl::Goal g;
interfaces::srv::JzCmd::Request r;
if (n && n.IsMap()) {
// geometry_msgs::msg::TwistStamped
if (n) {
if (n["frame_id"]) g.target.header.frame_id = n["frame_id"].as<std::string>();
if (n["stamp_sec"]) g.target.header.stamp.sec = n["stamp_sec"].as<int32_t>();
if (n["stamp_nanosec"]) g.target.header.stamp.nanosec = n["stamp_nanosec"].as<uint32_t>();
if (auto lin = n["linear"]) {
if (lin["x"]) g.target.twist.linear.x = lin["x"].as<double>();
if (lin["y"]) g.target.twist.linear.y = lin["y"].as<double>();
if (lin["z"]) g.target.twist.linear.z = lin["z"].as<double>();
}
if (auto ang = n["angular"]) {
if (ang["x"]) g.target.twist.angular.x = ang["x"].as<double>();
if (ang["y"]) g.target.twist.angular.y = ang["y"].as<double>();
if (ang["z"]) g.target.twist.angular.z = ang["z"].as<double>();
}
}
if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
if (n["loc"]) r.loc = n["loc"].as<uint8_t>();
if (n["speed"]) r.speed = n["speed"].as<uint8_t>();
if (n["torque"]) r.torque = n["torque"].as<uint8_t>();
}
return g;
return r;
}
// VisionGraspObject::Goal: object_id
// MotorInfo::Request: target, type, motor_ids
template<>
inline interfaces::action::VisionGraspObject::Goal from_yaml<interfaces::action::VisionGraspObject::Goal>(const YAML::Node & n)
inline interfaces::srv::MotorInfo::Request from_yaml<interfaces::srv::MotorInfo::Request>(const YAML::Node & n)
{
interfaces::action::VisionGraspObject::Goal g;
interfaces::srv::MotorInfo::Request r;
if (n && n.IsMap()) {
if (n["object_id"]) g.object_id = n["object_id"].as<std::string>();
if (n["target"]) r.target = n["target"].as<std::string>();
if (n["type"]) r.type = n["type"].as<std::string>();
if (n["motor_ids"]) {
auto vec = n["motor_ids"].as<std::vector<uint8_t>>();
r.motor_ids.assign(vec.begin(), vec.end());
}
}
return g;
return r;
}
// 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)
{
interfaces::srv::MotorParam::Request r;
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["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;
}
// BtRebuild::Request: type, config, param
template<>
inline interfaces::srv::BtRebuild::Request from_yaml<interfaces::srv::BtRebuild::Request>(const YAML::Node & n)
{
interfaces::srv::BtRebuild::Request r;
if (n && n.IsMap()) {
if (n["type"]) r.type = n["type"].as<std::string>();
if (n["config"]) r.config = n["config"].as<std::string>();
if (n["param"]) r.param = n["param"].as<std::string>();
}
return r;
}
} // namespace brain

21
src/brain/include/brain/skill_config.hpp
View File

@@ -7,6 +7,7 @@
#include "interfaces/action/arm.hpp"
#include "interfaces/action/camera_take_photo.hpp"
#include "interfaces/action/hand_control.hpp"
#include "interfaces/action/jz_cmd.hpp"
#include "interfaces/action/move_home.hpp"
#include "interfaces/action/move_leg.hpp"
#include "interfaces/action/move_waist.hpp"
@@ -30,7 +31,8 @@ using SkillActionTypes = std::tuple<
interfaces::action::MoveWaist,
interfaces::action::MoveLeg,
interfaces::action::MoveWheel,
interfaces::action::MoveHome
interfaces::action::MoveHome,
interfaces::action::JzCmd
>;
using SkillServiceTypes = std::tuple<
@@ -70,7 +72,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveWaist>
{
static constexpr const char * skill_name = "MoveWaist";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveWaist";
static bool success(const interfaces::action::MoveWaist::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveWaist::Result & r) {return r.message;}
};
@@ -79,7 +81,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveLeg>
{
static constexpr const char * skill_name = "MoveLeg";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveLeg";
static bool success(const interfaces::action::MoveLeg::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveLeg::Result & r) {return r.message;}
};
@@ -88,7 +90,7 @@ template<>
struct SkillActionTrait<interfaces::action::MoveWheel>
{
static constexpr const char * skill_name = "MoveWheel";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveWheel";
static bool success(const interfaces::action::MoveWheel::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveWheel::Result & r) {return r.message;}
};
@@ -97,11 +99,20 @@ template<>
struct SkillActionTrait<interfaces::action::MoveHome>
{
static constexpr const char * skill_name = "MoveHome";
static constexpr const char * default_topic = "";
static constexpr const char * default_topic = "MoveHome";
static bool success(const interfaces::action::MoveHome::Result & r) {return r.success;}
static std::string message(const interfaces::action::MoveHome::Result & r) {return r.message;}
};
template<>
struct SkillActionTrait<interfaces::action::JzCmd>
{
static constexpr const char * skill_name = "JzCmd";
static constexpr const char * default_topic = "";
static bool success(const interfaces::action::JzCmd::Result & r) {return (r.devid == 0);}
static std::string message(const interfaces::action::JzCmd::Result & r) {return r.state;}
};
template<>
struct SkillServiceTrait<interfaces::srv::VisionObjectRecognition>
{

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

@@ -129,8 +129,13 @@ void CerebellumNode::ConfigureArmHooks()
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 : 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) {
@@ -172,7 +177,6 @@ void CerebellumNode::ConfigureArmHooks()
return;
}
(void)feedback;
//TODO :
// RCLCPP_INFO(this->get_logger(), "[%s] command_id: %ld, current progress: %d",
// skill_name.c_str(), feedback->command_id, feedback->int_lenth);
@@ -184,11 +188,21 @@ void CerebellumNode::ConfigureArmHooks()
const std::string message = res.result ? std::string("action end") : std::string("No return information");
geometry_msgs::msg::Pose pose = res.result->pose;
RCLCPP_WARN(this->get_logger(), "[%s] Final position: %.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);
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] command_id %ld completed: %s", skill_name.c_str(), res.result->command_id, message.c_str());
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 {
@@ -480,6 +494,63 @@ void CerebellumNode::ConfigureMoveHomeHooks()
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;
}
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.
*
@@ -497,6 +568,7 @@ void CerebellumNode::ConfigureActionHooks()
ConfigureMoveLegHooks();
ConfigureMoveWheelHooks();
ConfigureMoveHomeHooks();
ConfigureJzCmdControlHooks();
}
/**
@@ -555,9 +627,9 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
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.21;
target_pose_[target_frame_].pose.position.z -= 0.19;
grab_width_ = obj.grab_width;
RCLCPP_INFO(this->get_logger(), "target_frame_: %s, updated target_pose", target_frame_.c_str());
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,
@@ -565,7 +637,7 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
const auto & p = obj.pose;
RCLCPP_INFO(this->get_logger(),
" pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
"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);
@@ -580,7 +652,7 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
skill_name.c_str(), arm_target_frame_.c_str());
RCLCPP_INFO(this->get_logger(),
" pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
"%.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,
@@ -588,35 +660,23 @@ void CerebellumNode::ConfigureVisionObjectRecognitionHooks()
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);
}
}
// const auto pose_it = target_pose_.find(target_frame_);
// if (pose_it != target_pose_.end()) {
// geometry_msgs::msg::PoseStamped pose_in_arm;
// const bool transformed = TransformPoseToArmFrame(pose_it->second, pose_in_arm);
// const auto & pose_ref = transformed ? pose_in_arm : pose_it->second;
// if (!transformed) {
// RCLCPP_WARN(this->get_logger(), "[%s] 坐标系转换失败，继续使用 %s 坐标系数据",
// skill_name.c_str(), pose_ref.header.frame_id.c_str());
// } else {
// RCLCPP_INFO(this->get_logger(), "[%s] 目标姿态已转换到 %s 坐标系",
// skill_name.c_str(), arm_target_frame_.c_str());
// RCLCPP_INFO(this->get_logger(),
// " pos(%.4f, %.4f, %.4f) quat(%.4f, %.4f, %.4f, %.4f)",
// pose_ref.pose.position.x,
// pose_ref.pose.position.y,
// pose_ref.pose.position.z,
// pose_ref.pose.orientation.x,
// pose_ref.pose.orientation.y,
// pose_ref.pose.orientation.z,
// pose_ref.pose.orientation.w);
// }
// } else {
// RCLCPP_WARN(this->get_logger(), "[%s] 未找到目标 %s 的姿态缓存，使用默认坐标",
// skill_name.c_str(), target_frame_.c_str());
// }
return true;
};
skill_manager_->configure_service_hooks<VisionSrv>("VisionObjectRecognition", std::move(hooks));
@@ -1228,9 +1288,32 @@ bool CerebellumNode::ExecuteActionSkill(
if (timeout_ms_override > 0) {
effective_timeout = std::chrono::milliseconds(timeout_ms_override);
}
return (skill == "Arm") ?
ExecuteBilateralArmAction(skill, topic, effective_timeout, instance_name, timeout_ms_override, index, total_skills, goal_handle, detail) :
RunActionSkillWithProgress(skill, topic, effective_timeout, instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
if (skill == "Arm") {
interfaces::action::Arm::Goal goal{};
if (!TryParseCallPayload<interfaces::action::Arm::Goal>(skill, goal)) {
RCLCPP_ERROR(this->get_logger(), "Failed to parse call payload for skill %s", skill.c_str());
return false;
}
if (goal.body_id >= 2) { //left and right
return ExecuteBilateralArmAction(skill, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
} else if (goal.body_id == 0 || goal.body_id == 1) { //left or right
tls_arm_body_id = goal.body_id;
} else {
RCLCPP_ERROR(this->get_logger(), "Invalid body_id: %d for skill %s", goal.body_id, skill.c_str());
return false;
}
}
return RunActionSkillWithProgress(skill, topic, effective_timeout,
instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
// return (skill == "Arm") ?
// ExecuteBilateralArmAction(skill, topic, effective_timeout,
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail) :
// RunActionSkillWithProgress(skill, topic, effective_timeout,
// instance_name, timeout_ms_override, index, total_skills, goal_handle, detail);
}
/**

16
src/scripts/gen_bt_params.py
View File

@@ -116,25 +116,25 @@ def parse_action_file(path: str) -> List[Tuple[str, str]]:
return fields
def locate_action_file(interfaces_root: str, base: str) -> Optional[str]:
action_dir = resolve_interface_subdir(interfaces_root, 'action')
def locate_action_file(interfaces_root: str, base: str, type: str) -> Optional[str]:
action_dir = resolve_interface_subdir(interfaces_root, type)
# 1) Try as-is (CamelCase)
candidate = os.path.join(action_dir, f'{base}.action')
candidate = os.path.join(action_dir, f'{base}.{type}')
if os.path.exists(candidate):
return candidate
# 2) Try CamelCase of snake
camel = snake_to_camel(base)
if camel != base:
candidate2 = os.path.join(action_dir, f'{camel}.action')
candidate2 = os.path.join(action_dir, f'{camel}.{type}')
if os.path.exists(candidate2):
return candidate2
# 3) Try snake_case
snake = to_snake(base)
candidate3 = os.path.join(action_dir, f'{snake}.action')
candidate3 = os.path.join(action_dir, f'{snake}.{type}')
if os.path.exists(candidate3):
return candidate3
# 4) Case-insensitive lookup
found = find_file_case_insensitive(action_dir, f'{base}.action')
found = find_file_case_insensitive(action_dir, f'{base}.{type}')
if found:
return found
# Not found
@@ -239,7 +239,9 @@ def main():
for instance_name, node_tag in nodes:
# Map tag to interface base by stripping trailing '_H' if present
base = node_tag[:-2] if node_tag.endswith('_H') else node_tag
action_file = locate_action_file(interfaces_root, base)
action_file = locate_action_file(interfaces_root, base, "action")
if action_file == None:
action_file = locate_action_file(interfaces_root, base, "srv")
fields: List[Tuple[str, str]] = parse_action_file(action_file) if action_file else []
yaml_params = build_yaml_params_from_fields(fields)
results.append({'name': instance_name, 'params': yaml_params})