modify JzCmd -> GripperCmd

src/brain/config/bt_vision_grasp.xml

@@ -4,14 +4,14 @@
       <!-- Retry vision + hand until GripperControl_H succeeds -->
       <RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
         <Sequence>
-          <!-- <JzCmd_H name="gripper_open" /> -->
+          <GripperCmd_H name="gripper_open" />
           <VisionObjectRecognition_H name="VisionObjectRecognition_H" />
-          <!-- <Arm_H name="right_arm_vision_grasp" /> -->
-          <!-- <JzCmd_H name="gripper_close" /> -->
+          <Arm_H name="right_arm_vision_grasp" />
+          <GripperCmd_H name="gripper_close" />
         </Sequence>
       </RetryUntilSuccessful>
       <!-- After Arm_H eventually succeeds, run the hand -->
-      <!-- <JzCmd_H name="JzCmd_H" /> -->
+      <!-- <GripperCmd_H name="GripperCmd_H" /> -->
     </Sequence>
   </BehaviorTree>
 </root>

src/brain/config/robot_skills.yaml

@@ -63,8 +63,9 @@
     - name: MoveHome.action
       default_topic: "MoveHome"
 
-- name: JzCmd
+- name: GripperCmd
   version: 1.0.0
   description: "夹爪控制"
   interfaces:
-    - JzCmd.action
+    - name: GripperCmd.action
+      default_topic: "gripper_cmd0"

src/brain/include/brain/cerebellum_hooks.hpp

@@ -22,7 +22,7 @@ public:
   static void ConfigureMoveLegHooks(CerebellumNode * node);
   static void ConfigureMoveWheelHooks(CerebellumNode * node);
   static void ConfigureMoveHomeHooks(CerebellumNode * node);
-  static void ConfigureJzCmdControlHooks(CerebellumNode * node);
+  static void ConfigureGripperCmdControlHooks(CerebellumNode * node);
 
   static void ConfigureServiceHooks(CerebellumNode * node);
   static void ConfigureVisionObjectRecognitionHooks(CerebellumNode * node);

src/brain/include/brain/payload_converters.hpp

@@ -10,8 +10,8 @@
 #include "interfaces/action/arm_space_control.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
 #include "interfaces/action/execute_bt_action.hpp"
+#include "interfaces/action/gripper_cmd.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"
@@ -21,7 +21,7 @@
 #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/gripper_cmd.hpp"
 #include "interfaces/srv/map_load.hpp"
 #include "interfaces/srv/map_save.hpp"
 #include "interfaces/srv/motor_info.hpp"
@@ -159,11 +159,11 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
   return g;
 }
 
-// JzCmd::Goal: loc, speed, torque, mode
+// GripperCmd::Goal: loc, speed, torque, mode
 template<>
-inline interfaces::action::JzCmd::Goal from_yaml<interfaces::action::JzCmd::Goal>(const YAML::Node & n)
+inline interfaces::action::GripperCmd::Goal from_yaml<interfaces::action::GripperCmd::Goal>(const YAML::Node & n)
 {
-  interfaces::action::JzCmd::Goal g;
+  interfaces::action::GripperCmd::Goal g;
   if (n && n.IsMap()) {
     if (n["loc"]) g.loc = n["loc"].as<uint8_t>();
     if (n["speed"]) g.speed = n["speed"].as<uint8_t>();
@@ -273,11 +273,11 @@ inline interfaces::action::ArmSpaceControl::Goal from_yaml<interfaces::action::A
   return g;
 }
 
-// JzCmd::Request: devid, loc, speed, torque
+// GripperCmd::Request: devid, loc, speed, torque
 template<>
-inline interfaces::srv::JzCmd::Request from_yaml<interfaces::srv::JzCmd::Request>(const YAML::Node & n)
+inline interfaces::srv::GripperCmd::Request from_yaml<interfaces::srv::GripperCmd::Request>(const YAML::Node & n)
 {
-  interfaces::srv::JzCmd::Request r;
+  interfaces::srv::GripperCmd::Request r;
   if (n && n.IsMap()) {
     if (n["devid"]) r.devid = n["devid"].as<uint8_t>();
     if (n["loc"]) r.loc = n["loc"].as<uint8_t>();

src/brain/include/brain/skill_config.hpp

@@ -6,8 +6,8 @@
 
 #include "interfaces/action/arm.hpp"
 #include "interfaces/action/camera_take_photo.hpp"
+#include "interfaces/action/gripper_cmd.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"
@@ -32,7 +32,7 @@ using SkillActionTypes = std::tuple<
   interfaces::action::MoveLeg,
   interfaces::action::MoveWheel,
   interfaces::action::MoveHome,
-  interfaces::action::JzCmd
+  interfaces::action::GripperCmd
 >;
 
 using SkillServiceTypes = std::tuple<
@@ -105,12 +105,12 @@ struct SkillActionTrait<interfaces::action::MoveHome>
 };
 
 template<>
-struct SkillActionTrait<interfaces::action::JzCmd>
+struct SkillActionTrait<interfaces::action::GripperCmd>
 {
-  static constexpr const char * skill_name = "JzCmd";
-  static constexpr const char * default_topic = "";
-  static bool success(const interfaces::action::JzCmd::Result & r) {return (r.result == 0);}
-  static std::string message(const interfaces::action::JzCmd::Result & r) {return r.state;}
+  static constexpr const char * skill_name = "GripperCmd";
+  static constexpr const char * default_topic = "gripper_cmd0";
+  static bool success(const interfaces::action::GripperCmd::Result & r) {return (r.result == 1);}
+  static std::string message(const interfaces::action::GripperCmd::Result & r) {return r.state;}
 };
 
 template<>

src/brain/src/cerebellum_hooks.cpp

@@ -20,7 +20,7 @@ void CerebellumHooks::ConfigureActionHooks(CerebellumNode * node)
   ConfigureMoveLegHooks(node);
   ConfigureMoveWheelHooks(node);
   ConfigureMoveHomeHooks(node);
-  ConfigureJzCmdControlHooks(node);
+  ConfigureGripperCmdControlHooks(node);
 }
 
 void CerebellumHooks::ConfigureArmSpaceControlHooks(CerebellumNode * node)
@@ -445,14 +445,14 @@ void CerebellumHooks::ConfigureMoveHomeHooks(CerebellumNode * node)
   node->skill_manager_->configure_action_hooks<interfaces::action::MoveHome>("MoveHome", std::move(hooks));
 }
 
-void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
+void CerebellumHooks::ConfigureGripperCmdControlHooks(CerebellumNode * node)
 {
-  // JzCmd
-  SkillManager::ActionHookOptions<interfaces::action::JzCmd> hooks;
+  // GripperCmd
+  SkillManager::ActionHookOptions<interfaces::action::GripperCmd> hooks;
   hooks.make_goal = [node](const std::string & skill_name) {
-      interfaces::action::JzCmd::Goal goal{};
+      interfaces::action::GripperCmd::Goal goal{};
       // Plan A: per-call YAML overrides
-      if (!node->TryParseCallPayload<interfaces::action::JzCmd::Goal>(skill_name, goal)) {
+      if (!node->TryParseCallPayload<interfaces::action::GripperCmd::Goal>(skill_name, goal)) {
         RCLCPP_ERROR(node->get_logger(), "Failed to parse call payload for skill %s", skill_name.c_str());
         return goal;
       }
@@ -466,8 +466,8 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
     };
   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) {
+      ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::SharedPtr,
+      const std::shared_ptr<const interfaces::action::GripperCmd::Feedback> & feedback) {
       if (!feedback) {return;}
 
       RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d", 
@@ -476,7 +476,7 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
     };
   hooks.on_result = [node](
       const std::string & skill_name,
-      const ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>::WrappedResult & res) {
+      const ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>::WrappedResult & res) {
 
       if (res.code == rclcpp_action::ResultCode::CANCELED) {
         RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
@@ -490,14 +490,14 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
     };
   hooks.on_goal_response = [node](
       const std::string & skill_name,
-      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::JzCmd>> & handle) {
+      const std::shared_ptr<ActionClientRegistry::GoalHandle<interfaces::action::GripperCmd>> & 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));
+  node->skill_manager_->configure_action_hooks<interfaces::action::GripperCmd>("GripperCmd", std::move(hooks));
 }
 
 void CerebellumHooks::ConfigureServiceHooks(CerebellumNode * node)
@@ -568,7 +568,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
             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.14;
+            node->target_pose_[node->target_frame_].pose.position.x -=  obj.grab_width/2;
+            // 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_);
           }
@@ -610,8 +611,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
                 target_pos, 
                 target_quat, 
                 "side", 
-                0.06, // safety_dist
-                0.19   // finger_length
+                0.07, // safety_dist
+                0.17   // finger_length
             );
 
             geometry_msgs::msg::Pose pre_grasp_msg;
@@ -659,8 +660,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
                 target_pos, 
                 target_quat, 
                 "top", 
-                0.06, // safety_dist
-                0.19   // finger_length
+                0.02, // safety_dist
+                0.0   // finger_length
             );
 
             pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();