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

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

@ -332,6 +332,7 @@ class STAGRecognizer:
Returns: Returns:
_type_: _description_ _type_: _description_
""" """
try:
if self.current_frame is None: if self.current_frame is None:
rospy.logwarn("No image received yet") rospy.logwarn("No image received yet")
return [] return []
@ -343,10 +344,13 @@ class STAGRecognizer:
marker_pos_pack = self.calc_markers_base_position(corners, ids) marker_pos_pack = self.calc_markers_base_position(corners, ids)
if len(marker_pos_pack) == 0 and not rospy.is_shutdown(): if len(marker_pos_pack) == 0 and not rospy.is_shutdown():
# rospy.logwarn("No markers detected") # rospy.logwarn("No markers detected")
marker_pos_pack = self.stag_identify() 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)