add take photo pose

src/brain/config/bt_vision_grasp.params.yaml

@@ -16,11 +16,11 @@
     mode: 2
 
     '
-- name: s2_VisionObjectRecognition_H
-  params: 'camera_position: right
+- name: s2_top_camera_vision_recg
+  params: 'camera_position: top
 
     '
-- name: s3_right_arm_vision_pre_grasp
+- name: s2_right_arm_cam_take_photo
   params: 'body_id: 1
 
     data_type: 100
@@ -34,7 +34,11 @@
     data_array: []
 
     '
-- name: s4_right_arm_vision_grasp
+- name: s3_right_camera_vision_recg
+  params: 'camera_position: right
+
+    '
+- name: s3_right_arm_vision_pre_grasp
   params: 'body_id: 1
 
     data_type: 101
@@ -47,11 +51,25 @@
 
     data_array: []
 
+    '
+- name: s4_right_arm_vision_grasp
+  params: 'body_id: 1
+
+    data_type: 102
+
+    data_length: 0
+
+    command_id: 0
+
+    frame_time_stamp: 0
+
+    data_array: []
+
     '
 - name: s5_right_arm_vision_pre_grasp
   params: 'body_id: 1
 
-    data_type: 100
+    data_type: 101
 
     data_length: 0
 
@@ -65,7 +83,7 @@
 - name: s6_right_arm_cam_take_photo
   params: 'body_id: 1
 
-    data_type: 90
+    data_type: 100
 
     data_length: 0
 
@@ -79,7 +97,7 @@
 - name: s7_right_arm_vision_pre_grasp
   params: 'body_id: 1
 
-    data_type: 102
+    data_type: 103
 
     data_length: 0
 
@@ -93,7 +111,7 @@
 - name: s8_right_arm_vision_grasp
   params: 'body_id: 1
 
-    data_type: 103
+    data_type: 104
 
     data_length: 0
 
@@ -107,7 +125,7 @@
 - name: s9_right_arm_vision_pre_grasp
   params: 'body_id: 1
 
-    data_type: 102
+    data_type: 103
 
     data_length: 0
 

src/brain/config/bt_vision_grasp.xml

@@ -5,7 +5,9 @@
       <RetryUntilSuccessful name="retry_vision_hand" num_attempts="3">
         <Sequence>
           <!-- <GripperCmd_H name="s1_gripper_open" /> -->
-          <VisionObjectRecognition_H name="s2_VisionObjectRecognition_H" />
+          <VisionObjectRecognition_H name="s2_top_camera_vision_recg" />
+          <Arm_H name="s2_right_arm_cam_take_photo" />
+          <VisionObjectRecognition_H name="s3_right_camera_vision_recg" />
           <Arm_H name="s3_right_arm_vision_pre_grasp" />
           <Arm_H name="s4_right_arm_vision_grasp" />
           <Arm_H name="s5_right_arm_vision_pre_grasp" />

src/brain/include/brain/cerebellum_node.hpp

@@ -43,6 +43,7 @@ public:
    */
   explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
+  geometry_msgs::msg::Pose right_hand_take_photo_pose_;
   std::vector<geometry_msgs::msg::Pose> pre_grasp_poses_;
   std::vector<geometry_msgs::msg::Pose> grasp_poses_;
 
@@ -51,6 +52,8 @@ public:
   std::mutex joint_state_mutex_;
   std::vector<double> right_arm_cam_joint_angles_;
 
+  std::string camera_position_;
+
 private:
   struct GoalExecutionContext { std::atomic<bool> cancel{false}; };
   std::unordered_map<std::uintptr_t, std::shared_ptr<GoalExecutionContext>> goal_ctxs_;

src/brain/src/calc_grasp_pose.cpp

@@ -117,7 +117,8 @@ GraspResult GraspPoseCalculator::calculate(
     
     GraspResult result = create_grasp_matrix(target_pos, approach_vec, palm_normal, safety_dist, finger_length);
     result.name = name_suffix;
-
+    
+#if ENABLE_TOLERANCE
     // Check IK and adjust position if needed (Tolerance +/- 2cm)
     std::vector<double> q_out(7, 0.0);
     // Try original position first
@@ -152,6 +153,7 @@ GraspResult GraspPoseCalculator::calculate(
              RCLCPP_WARN(rclcpp::get_logger("GraspPoseCalculator"), "Could not find valid IK within tolerance.");
         }
     }
+#endif
 
     return result;
 }

src/brain/src/cerebellum_hooks.cpp

@@ -84,13 +84,16 @@ void CerebellumHooks::ConfigureArmHooks(CerebellumNode * node)
         goal.data_length = USED_ARM_DOF;
       } else if (goal.data_type >= 100 && goal.data_type < 110) {
         geometry_msgs::msg::Pose pose;
-        if (goal.data_type == 100) { // pre-grasp
+        if (goal.data_type == 100) {
+          pose = node->right_hand_take_photo_pose_;
+        }
+        else if (goal.data_type == 101) { // pre-grasp
           pose = node->pre_grasp_poses_[0];
-        } else if (goal.data_type == 101) { // grasp
+        } else if (goal.data_type == 102) { // grasp
           pose = node->grasp_poses_[0];
-        } else if (goal.data_type == 102) { // left arm pre-grasp
+        } else if (goal.data_type == 103) { // left arm pre-grasp
           pose = node->pre_grasp_poses_[1];
-        } else if (goal.data_type == 103) { // left arm grasp
+        } else if (goal.data_type == 104) { // left arm grasp
           pose = node->grasp_poses_[1];
         } else {
           RCLCPP_ERROR(node->get_logger(), "ARM make goal error: invalid data_type");
@@ -554,7 +557,8 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
         node->target_pose_[tf].header.frame_id = "RightLink6";
         node->target_pose_[tf].pose = obj.pose;
         node->grab_width_.clear();
-        for (int i = 0; i < obj.grab_width.size(); i++) {
+        int size = (int)(obj.grab_width.size());
+        for (int i = 0; i < size; i++) {
           node->grab_width_.push_back(obj.grab_width[i]);
           RCLCPP_INFO(node->get_logger(), "grab_width : %.4f", obj.grab_width[i]);
         }
@@ -576,6 +580,37 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
     // RCLCPP_INFO(node->get_logger(), "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);
 
+    if (node->camera_position_ == "top") {
+      geometry_msgs::msg::Pose pose = node->target_pose_[node->target_frame_].pose;
+      RCLCPP_INFO(node->get_logger(), "[%s] target pose %s frame, camera_position: %s", 
+        skill_name.c_str(), node->arm_target_frame_.c_str(), node->camera_position_.c_str());
+      RCLCPP_INFO(node->get_logger(), "target pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", 
+        pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, 
+        pose.orientation.y, pose.orientation.z, pose.orientation.w);
+
+      node->right_hand_take_photo_pose_.position.x = pose.position.x + 0.04f;
+      node->right_hand_take_photo_pose_.position.y = pose.position.y - 0.35f;
+      node->right_hand_take_photo_pose_.position.z = pose.position.z;
+      node->right_hand_take_photo_pose_.orientation.x = -0.7071;
+      node->right_hand_take_photo_pose_.orientation.y = 0.0000;
+      node->right_hand_take_photo_pose_.orientation.z = -0.0000;
+      node->right_hand_take_photo_pose_.orientation.w = 0.7071;
+
+      RCLCPP_INFO(node->get_logger(), "cam take photo pose: %.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f", 
+        node->right_hand_take_photo_pose_.position.x, node->right_hand_take_photo_pose_.position.y, 
+        node->right_hand_take_photo_pose_.position.z, node->right_hand_take_photo_pose_.orientation.x, 
+        node->right_hand_take_photo_pose_.orientation.y, node->right_hand_take_photo_pose_.orientation.z, 
+        node->right_hand_take_photo_pose_.orientation.w);
+
+      if (node->right_hand_take_photo_pose_.position.x < 0.1 || 
+        node->right_hand_take_photo_pose_.position.y < 0.1 || node->right_hand_take_photo_pose_.position.z < 0.1) {
+          RCLCPP_ERROR(node->get_logger(), "invalid pose value, cal grasp pose failed");
+          return false;
+      }
+
+      return true;
+    }
+
     geometry_msgs::msg::PoseStamped pose_in_arm;
     const bool transformed = node->TransformPoseToArmFrame(node->target_pose_[node->target_frame_], pose_in_arm);
     if (!transformed) {
@@ -620,15 +655,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
             auto result = brain::GraspPoseCalculator::calculate(target_pos, 
               target_quat, "side", angle, palm, 0.10, 0.20, node->arm_target_frame_);
 
-            geometry_msgs::msg::Pose pre_grasp_msg;
-            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
-            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
-            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
-            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
-            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
-            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
-            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
-
             std::vector<double> pre_grasp_joint_angles = current_joint_angles;
             std::vector<double> grasp_joint_angles = current_joint_angles;
 
@@ -646,14 +672,14 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
               continue;
             }
 
-            geometry_msgs::msg::Pose grasp_msg;
-            grasp_msg.position.x = result.grasp_pose.position.x();
-            grasp_msg.position.y = result.grasp_pose.position.y();
-            grasp_msg.position.z = result.grasp_pose.position.z();
-            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
-            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
-            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
-            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
+            geometry_msgs::msg::Pose pre_grasp_msg;
+            pre_grasp_msg.position.x = result.pre_grasp_pose.position.x();
+            pre_grasp_msg.position.y = result.pre_grasp_pose.position.y();
+            pre_grasp_msg.position.z = result.pre_grasp_pose.position.z();
+            pre_grasp_msg.orientation.x = result.pre_grasp_pose.orientation.x();
+            pre_grasp_msg.orientation.y = result.pre_grasp_pose.orientation.y();
+            pre_grasp_msg.orientation.z = result.pre_grasp_pose.orientation.z();
+            pre_grasp_msg.orientation.w = result.pre_grasp_pose.orientation.w();
 
 #ifdef DEBUG_GRASP_POSE_CALCULATION
             RCLCPP_INFO(node->get_logger(), "[%s] angle %f, palm %s, grasp_pose[%.4f, %.4f, %.4f, %.4f, %.4f, %.4f, %.4f]", 
@@ -669,6 +695,15 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
               continue;
             }
 
+            geometry_msgs::msg::Pose grasp_msg;
+            grasp_msg.position.x = result.grasp_pose.position.x();
+            grasp_msg.position.y = result.grasp_pose.position.y();
+            grasp_msg.position.z = result.grasp_pose.position.z();
+            grasp_msg.orientation.x = result.grasp_pose.orientation.x();
+            grasp_msg.orientation.y = result.grasp_pose.orientation.y();
+            grasp_msg.orientation.z = result.grasp_pose.orientation.z();
+            grasp_msg.orientation.w = result.grasp_pose.orientation.w();
+
             node->pre_grasp_poses_.push_back(pre_grasp_msg);
             node->grasp_poses_.push_back(grasp_msg);
 
@@ -707,6 +742,7 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
         //   node->declare_parameter<std::string>(param, std::string("right"));
         // }
         // req->camera_position = node->get_parameter(param).as_string();
+        node->camera_position_ = req->camera_position;
         RCLCPP_INFO(node->get_logger(), "[%s] camera_position: %s", skill_name.c_str(), req->camera_position.c_str());
 
         return req;
@@ -728,7 +764,6 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
         RCLCPP_INFO(node->get_logger(), "[%s] success=%d classes=%zu frame_id=%s stamp=%.3f info=%s",
           skill_name.c_str(), resp->success, resp->objects.size(), 
           resp->header.frame_id.c_str(), stamp_sec, resp->info.c_str());
-
         node->pre_grasp_poses_.clear();
         node->grasp_poses_.clear();
 
@@ -745,14 +780,16 @@ void CerebellumHooks::ConfigureVisionObjectRecognitionHooks(CerebellumNode * nod
 #endif
         }
 
-        if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
-          RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
-          return false;
-        }
-        if (node->pre_grasp_poses_.size() != node->grasp_poses_.size()) {
-          RCLCPP_WARN(node->get_logger(), "[%s] pre_grasp_poses_ size %zu not match grasp_poses_ size %zu",
-            skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
-          return false;
+        if (node->camera_position_ != "top") {
+          if (node->pre_grasp_poses_.empty() || node->grasp_poses_.empty()) {
+            RCLCPP_WARN(node->get_logger(), "[%s] no valid grasp poses generated", skill_name.c_str());
+            return false;
+          }
+          if (node->pre_grasp_poses_.size() != node->grasp_poses_.size()) {
+            RCLCPP_WARN(node->get_logger(), "[%s] pre_grasp_poses_ size %zu not match grasp_poses_ size %zu",
+              skill_name.c_str(), node->pre_grasp_poses_.size(), node->grasp_poses_.size());
+            return false;
+          }
         }
 
         node->right_arm_cam_joint_angles_.clear();