kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-14 09:00:53 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

update detect_stag.py

Browse source
This commit is contained in:
wangWking 2024-09-23 15:37:55 +08:00
parent 8cc97563ef
commit 8f8cef31d0
2 changed files with 27 additions and 16 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
mycobot_280/mycobot_280/config/mycobot_with_marker.rviz
View file

@ -154,10 +154,10 @@ Visualization Manager:
Value: true Value: true
- Class: rviz/Marker - Class: rviz/Marker
Enabled: true Enabled: true
Marker Topic: /cube Marker Topic: cube
Name: Marker Name: Marker
Namespaces: Namespaces:
cube: true {}
Queue Size: 100 Queue Size: 100
Value: true Value: true
Enabled: true Enabled: true

39
mycobot_280/mycobot_280/scripts/detect_stag.py
View file

@ -332,21 +332,25 @@ class STAGRecognizer:
Returns: Returns:
_type_: _description_ _type_: _description_
""" """
if self.current_frame is None: try:
rospy.logwarn("No image received yet") if self.current_frame is None:
return [] rospy.logwarn("No image received yet")
# 获取当前帧 return []
frame = self.current_frame # 获取当前帧
# 获取画面中二维码的角度和id frame = self.current_frame
corners, ids, rejected_corners = stag.detectMarkers(frame, 11) # 获取画面中二维码的角度和id
# 获取物的坐标(相机系) corners, ids, rejected_corners = stag.detectMarkers(frame, 11)
marker_pos_pack = self.calc_markers_base_position(corners, ids) # 获取物的坐标(相机系)
if len(marker_pos_pack) == 0 and not rospy.is_shutdown(): marker_pos_pack = self.calc_markers_base_position(corners, ids)
# rospy.logwarn("No markers detected") if len(marker_pos_pack) == 0 and not rospy.is_shutdown():
marker_pos_pack = self.stag_identify() # rospy.logwarn("No markers detected")
marker_pos_pack = self.stag_identify() # 递归调用
# print("Camera coords = ", marker_pos_pack) # print("Camera coords = ", marker_pos_pack)
return marker_pos_pack return marker_pos_pack
except RecursionError:
# rospy.logerr("Recursion depth exceeded in marker detection")
return [0, 0, 0, 0] # 返回默认值
def vision_trace(self, mode, ml): def vision_trace(self, mode, ml):
sp = 40 sp = 40
@ -369,6 +373,10 @@ class STAGRecognizer:
def stag_robot_identify(self): def stag_robot_identify(self):
marker_pos_pack = self.stag_identify() marker_pos_pack = self.stag_identify()
# 如果返回的是默认值，直接退出函数，不返回任何数据
if marker_pos_pack == [0, 0, 0, 0]:
rospy.logwarn("No markers detected, skipping processing")
return None # 直接返回 None
target_coords = self.get_coords() target_coords = self.get_coords()
while len(target_coords)==0: while len(target_coords)==0:
target_coords = self.get_coords() target_coords = self.get_coords()
@ -401,6 +409,9 @@ class STAGRecognizer:
rate = rospy.Rate(30) rate = rospy.Rate(30)
while not rospy.is_shutdown(): while not rospy.is_shutdown():
target_coords = self.stag_robot_identify() target_coords = self.stag_robot_identify()
# 如果没有返回目标坐标，跳过本次循环
if target_coords is None:
continue # 跳过这次循环，等下次识别
target_coords[0] -= 300 target_coords[0] -= 300
rospy.loginfo('Target Coords: %s', target_coords) rospy.loginfo('Target Coords: %s', target_coords)
self.coord_limit(target_coords) self.coord_limit(target_coords)