launching a unified behavior tree (+rbss)
This commit is contained in:
parent
4c64536b80
commit
cf692ff4c1
8 changed files with 460 additions and 325 deletions
|
|
@ -41,9 +41,11 @@ class ObjectDetection(Node):
|
|||
"""Construct the node."""
|
||||
self._count: int = 0
|
||||
self._pub: Optional[Publisher] = None
|
||||
self._pubI: Optional[Publisher] = None
|
||||
self._timer: Optional[Timer] = None
|
||||
self._image_cnt: int = 0
|
||||
self._sub = None
|
||||
self._threshold = 0.0
|
||||
self._is_image_mode = False
|
||||
self.yolov8_weights_file = ""
|
||||
self.model = None
|
||||
|
|
@ -75,14 +77,15 @@ class ObjectDetection(Node):
|
|||
|
||||
def _Settings(self):
|
||||
# Initialization service settings
|
||||
# TODO
|
||||
# for prop in self.skill_cfg["Settings"]:
|
||||
# nam = prop["name"]
|
||||
# val = prop["value"]
|
||||
# if nam == "publishDelay":
|
||||
# self.publishDelay = val
|
||||
# elif nam == "is_image_mode":
|
||||
# self._is_image_mode = val
|
||||
for prop in self.skill_cfg["Settings"]["output"]["params"]:
|
||||
nam = prop["name"]
|
||||
val = prop["value"]
|
||||
if nam == "threshold":
|
||||
self._threshold = float(val)
|
||||
elif nam == "publishDelay":
|
||||
self.publishDelay = float(val)
|
||||
elif nam == "is_image_mode":
|
||||
self._is_image_mode = (val == "True")
|
||||
|
||||
for prop in self.skill_cfg["topicsOut"]:
|
||||
if prop["type"] == OUT_TOPIC_TYPE:
|
||||
|
|
@ -133,10 +136,9 @@ class ObjectDetection(Node):
|
|||
return TransitionCallbackReturn.FAILURE
|
||||
|
||||
# Create the publisher.
|
||||
self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10)
|
||||
if self._is_image_mode:
|
||||
self._pub = self.create_lifecycle_publisher(Image, self.topicDetectImage, 3)
|
||||
else:
|
||||
self._pub = self.create_lifecycle_publisher(BoundBox, self.topicSrv, 10)
|
||||
self._pubI = self.create_lifecycle_publisher(Image, self.topicDetectImage, 1)
|
||||
|
||||
self._timer = self.create_timer(self.publishDelay, self.publish)
|
||||
|
||||
|
|
@ -149,7 +151,7 @@ class ObjectDetection(Node):
|
|||
def on_activate(self, state: LifecycleState) -> TransitionCallbackReturn:
|
||||
self.get_logger().info('on_activate() is called.')
|
||||
# Create the main subscriber.
|
||||
self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 3)
|
||||
self._sub = self.create_subscription(Image, self.topicImage, self.listener_callback, 1)
|
||||
return super().on_activate(state)
|
||||
|
||||
def on_deactivate(self, state: LifecycleState) -> TransitionCallbackReturn:
|
||||
|
|
@ -176,6 +178,7 @@ class ObjectDetection(Node):
|
|||
|
||||
self.destroy_timer(self._timer)
|
||||
self.destroy_publisher(self._pub)
|
||||
self.destroy_publisher(self._pubI)
|
||||
self.destroy_subscription(self._sub)
|
||||
|
||||
self.get_logger().info('on_cleanup() is called.')
|
||||
|
|
@ -187,6 +190,7 @@ class ObjectDetection(Node):
|
|||
"""
|
||||
self.destroy_timer(self._timer)
|
||||
self.destroy_publisher(self._pub)
|
||||
self.destroy_publisher(self._pubI)
|
||||
self.destroy_subscription(self._sub)
|
||||
|
||||
self.get_logger().info('on_shutdown() is called.')
|
||||
|
|
@ -198,36 +202,38 @@ class ObjectDetection(Node):
|
|||
|
||||
if self._pub is not None and self._pub.is_activated:
|
||||
# Publish result
|
||||
if not self.bbox_res:
|
||||
return
|
||||
msg = BoundBox()
|
||||
msg.is_detection = False
|
||||
#from ultralytics.engine.results
|
||||
cconf = self._threshold # threshold
|
||||
bb = None
|
||||
for c in self.bbox_res.boxes:
|
||||
idx = int(c.cls)
|
||||
if self.bbox_res.names[idx] == self.objName:
|
||||
conf = float(c.conf)
|
||||
if cconf < conf:
|
||||
cconf = conf
|
||||
bb = c
|
||||
if bb:
|
||||
msg.is_detection = True
|
||||
msg.name = self.objName
|
||||
msg.x = float(bb.xywhn[0,0])
|
||||
msg.y = float(bb.xywhn[0,1])
|
||||
msg.w = float(bb.xywhn[0,2])
|
||||
msg.h = float(bb.xywhn[0,3])
|
||||
msg.conf = float(bb.conf)
|
||||
# Publish the message.
|
||||
self._pub.publish(msg)
|
||||
|
||||
if self._is_image_mode:
|
||||
if len(self.image_det) == 0:
|
||||
return
|
||||
# The 'cv2_to_imgmsg' method converts an OpenCV image to a ROS 2 image message
|
||||
msg = self.br.cv2_to_imgmsg(self.image_det, encoding="bgr8")
|
||||
else:
|
||||
if not self.bbox_res:
|
||||
return
|
||||
msg = BoundBox()
|
||||
msg.is_detection = False
|
||||
#from ultralytics.engine.results
|
||||
cconf = 0.0 # threshold
|
||||
bb = None
|
||||
for c in self.bbox_res.boxes:
|
||||
idx = int(c.cls)
|
||||
if self.bbox_res.names[idx] == self.objName:
|
||||
conf = float(c.conf)
|
||||
if cconf < conf:
|
||||
cconf = conf
|
||||
bb = c
|
||||
if bb:
|
||||
msg.is_detection = True
|
||||
msg.name = self.objName
|
||||
msg.x = float(bb.xywhn[0,0])
|
||||
msg.y = float(bb.xywhn[0,1])
|
||||
msg.w = float(bb.xywhn[0,2])
|
||||
msg.h = float(bb.xywhn[0,3])
|
||||
msg.conf = float(bb.conf)
|
||||
# Publish the message.
|
||||
self._pub.publish(msg)
|
||||
# Publish the message.
|
||||
self._pubI.publish(msg)
|
||||
|
||||
def listener_callback(self, data):
|
||||
"""
|
||||
|
|
@ -235,7 +241,7 @@ class ObjectDetection(Node):
|
|||
"""
|
||||
if self.model:
|
||||
self._image_cnt += 1
|
||||
if self._image_cnt % 100 == 1:
|
||||
if self._image_cnt % 200 == 1:
|
||||
self.get_logger().info(f"detection: {self._image_cnt}")
|
||||
|
||||
# Convert ROS Image message to OpenCV image
|
||||
|
|
@ -243,12 +249,12 @@ class ObjectDetection(Node):
|
|||
# run inference
|
||||
results = self.model(current_frame)
|
||||
|
||||
for r in results:
|
||||
self.bbox_res = r
|
||||
|
||||
if self._is_image_mode:
|
||||
for r in results:
|
||||
self.image_det = r.plot() # plot a BGR numpy array of predictions
|
||||
else:
|
||||
for r in results:
|
||||
self.bbox_res = r
|
||||
|
||||
# self.get_logger().info(f"detection: end {self._image_cnt}")
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue