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

修复使用穹彻技能包抓取宽度数据

Browse Source
This commit is contained in:
liangyuxuan
2025-11-24 11:16:59 +08:00
parent 9998c52b60
commit 08c55b8132
1 changed files with 67 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

99
vision_detect/vision_detect/flexivaidk_detect_service.py
View File

@@ -325,6 +325,7 @@ class DetectNode(Node):
rgb_crop, depth_crop, mask_crop, (x_min, y_min) = crop_mask_bbox(rgb_img, depth_img, mask, box)
if depth_crop is None:
self.get_logger().error("depth_crop is None")
continue
depth_img_crop_mask = np.zeros_like(depth_crop)
@@ -362,41 +363,75 @@ class DetectNode(Node):
f"current detected object names: {self.aidk_client.get_detected_obj_names()}, current detected object nums: {self.aidk_client.get_detected_obj_nums()}"
)
for key in self.config["keys"]:
parse_state, result_list = self.aidk_client.parse_result(self.config["command"]["obj_name"], key, -1)
self.get_logger().info(
"detected time stamp: {}".format(
datetime.fromtimestamp(self.aidk_client.get_detected_time())
)
self.get_logger().info(
"detected time stamp: {}".format(
datetime.fromtimestamp(self.aidk_client.get_detected_time())
)
if not parse_state:
self.get_logger().error("Parse result error!!!")
continue
else:
if key in ["bbox", "keypoints", "positions", "obj_pose"]:
for result in result_list:
self.get_logger().info(f'"double_value in result": {getattr(result, "double_value")}')
for vec in result.vect:
self.get_logger().info(f"vec: {vec}")
x, y, z, rw, rx, ry, rz = vec
)
pose = Pose()
pose.position = Point(x=x, y=y, z=z)
pose.orientation = Quaternion(w=rw, x=rx, y=ry, z=rz)
pose_list.append(
{
"class_id": int(class_ids[i]),
"class_name": labels[class_ids[i]],
"pose": pose,
"grab_width": getattr(result, "double_value")
}
)
self.get_logger().info(f"pose: position({x}, {y}, {z}), orientation({rw}, {rx}, {ry}, {rz})")
self.get_logger().info(f"class_id: {int(class_ids[i])}, class_name: {labels[class_ids[i]]}, grab_width: {getattr(result, 'double_value')}")
parse_state, result_list_grab_width = self.aidk_client.parse_result(self.config["command"]["obj_name"], "double_value", -1)
if not parse_state:
self.get_logger().error("Parse result error!!!")
continue
elif key in ["valid", "double_value", "int_value", "name"]:
for result in result_list:
self.get_logger().info(f"{key}: {getattr(result, key)}")
parse_state, result_list_obj_pose = self.aidk_client.parse_result(self.config["command"]["obj_name"], "obj_pose", -1)
if not parse_state:
self.get_logger().error("Parse result error!!!")
continue
for result_pose, result_width in zip(result_list_obj_pose, result_list_grab_width):
self.get_logger().info(f'"double_value": {getattr(result_width, "double_value")}')
for vec in result_pose.vect:
self.get_logger().info(f"vec: {vec}")
x, y, z, rw, rx, ry, rz = vec
pose = Pose()
pose.position = Point(x=x, y=y, z=z)
pose.orientation = Quaternion(w=rw, x=rx, y=ry, z=rz)
pose_list.append(
{
"class_id": int(class_ids[i]),
"class_name": labels[class_ids[i]],
"pose": pose,
"grab_width": getattr(result_width, "double_value")
}
)
# for key in self.config["keys"]:
# parse_state, result_list = self.aidk_client.parse_result(self.config["command"]["obj_name"], key, -1)
# self.get_logger().info(
# "detected time stamp: {}".format(
# datetime.fromtimestamp(self.aidk_client.get_detected_time())
# )
# )
# if not parse_state:
# self.get_logger().error("Parse result error!!!")
# continue
# else:
# if key in ["bbox", "keypoints", "positions", "obj_pose"]:
# for result in result_list:
# self.get_logger().info(f'"double_value in result": {getattr(result, "double_value")}')
# for vec in result.vect:
# self.get_logger().info(f"vec: {vec}")
# x, y, z, rw, rx, ry, rz = vec
#
# pose = Pose()
# pose.position = Point(x=x, y=y, z=z)
# pose.orientation = Quaternion(w=rw, x=rx, y=ry, z=rz)
# pose_list.append(
# {
# "class_id": int(class_ids[i]),
# "class_name": labels[class_ids[i]],
# "pose": pose,
# "grab_width": getattr(result, "double_value")
# }
# )
# self.get_logger().info(f"pose: position({x}, {y}, {z}), orientation({rw}, {rx}, {ry}, {rz})")
# self.get_logger().info(f"class_id: {int(class_ids[i])}, class_name: {labels[class_ids[i]]}, grab_width: {getattr(result, 'double_value')}")
#
# elif key in ["valid", "double_value", "int_value", "name"]:
# for result in result_list:
# self.get_logger().info(f"{key}: {getattr(result, key)}")
time4 = time.time()