更新夹爪参数

src/brain/config/bt_vision_grasp.params.yaml

@@ -7,9 +7,7 @@
 
     '
 - name: gripper_open
-  params: 'devid: 6
-
-    loc: 0
+  params: 'loc: 0
 
     speed: 20
 
@@ -37,9 +35,7 @@
 
     '
 - name: gripper_close
-  params: 'devid: 6
-
-    loc: 30
+  params: 'loc: 30
 
     speed: 20
 

src/brain/config/bt_vision_grasp.xml

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

src/brain/include/brain/payload_converters.hpp

@@ -159,13 +159,12 @@ inline interfaces::action::MoveHome::Goal from_yaml<interfaces::action::MoveHome
   return g;
 }
 
-// JzCmd::Goal: devid, loc, speed, torque, mode
+// JzCmd::Goal: 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>();

src/brain/include/brain/skill_config.hpp

@@ -109,7 +109,7 @@ 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 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;}
 };
 

src/brain/src/cerebellum_hooks.cpp

@@ -459,8 +459,8 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
 
       // goal.loc = 30; //grab_width_; //TODO transfer
 
-      RCLCPP_INFO(node->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);
+      RCLCPP_INFO(node->get_logger(), "[%s] action make_goal params, loc: %d, speed: %d, torque: %d, mode: %d", 
+        skill_name.c_str(), goal.loc, goal.speed, goal.torque, goal.mode);
 
       return goal;
     };
@@ -470,8 +470,8 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
       const std::shared_ptr<const interfaces::action::JzCmd::Feedback> & feedback) {
       if (!feedback) {return;}
 
-      RCLCPP_INFO(node->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);
+      RCLCPP_INFO(node->get_logger(), "[%s] action feedback, loc: %d, speed: %d, torque: %d", 
+        skill_name.c_str(), feedback->loc, feedback->speed, feedback->torque);
 
     };
   hooks.on_result = [node](
@@ -481,8 +481,8 @@ void CerebellumHooks::ConfigureJzCmdControlHooks(CerebellumNode * node)
       if (res.code == rclcpp_action::ResultCode::CANCELED) {
         RCLCPP_WARN(node->get_logger(), "[%s] was cancelled", skill_name.c_str());
       } else if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
-        RCLCPP_INFO(node->get_logger(), "[%s] action on_result, devid: %d, state: %s", 
-          skill_name.c_str(), res.result->devid, res.result->state.c_str());
+        RCLCPP_INFO(node->get_logger(), "[%s] action on_result, state: %s", 
+          skill_name.c_str(), res.result->state.c_str());
       } else {
         RCLCPP_ERROR(node->get_logger(), "[%s] was failed(code=%d)", skill_name.c_str(), static_cast<int>(res.code));
       }
@@ -568,7 +568,7 @@ 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.19;
+            // node->target_pose_[node->target_frame_].pose.position.z -= 0.14;
             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_);
           }
@@ -611,7 +611,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
                 target_quat, 
                 "side", 
                 0.06, // safety_dist
-                0.0   // finger_length
+                0.19   // finger_length
             );
 
             geometry_msgs::msg::Pose pre_grasp_msg;
@@ -660,7 +660,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
                 target_quat, 
                 "top", 
                 0.06, // safety_dist
-                0.0   // finger_length
+                0.19   // finger_length
             );
 
             pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();