From 29fe2190afa4f53c74afb21e9d32e68e89963c36 Mon Sep 17 00:00:00 2001
From: Hirokazu Ishida <38597814+HiroIshida@users.noreply.github.com>
Date: Wed, 6 Apr 2022 02:55:01 +0900
Subject: [PATCH] As a server (#22)
* Define custom service
* Add request handler
* Turn off pubsub
* Add client example
* Enable loading custom image
---
CMakeLists.txt | 5 +++++
example/client.py | 28 ++++++++++++++++++++++++++++
launch/sample.launch | 2 ++
node_script/node.py | 25 ++++++++++++++++++++-----
node_script/node_config.py | 4 ++++
srv/DeticSeg.srv | 4 ++++
6 files changed, 63 insertions(+), 5 deletions(-)
create mode 100755 example/client.py
create mode 100644 srv/DeticSeg.srv
diff --git a/CMakeLists.txt b/CMakeLists.txt
index f3865a8..df97485 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,6 +19,11 @@ add_message_files(
SegmentationInfo.msg
)
+add_service_files(
+ FILES
+ DeticSeg.srv
+)
+
generate_messages(
DEPENDENCIES
std_msgs
diff --git a/example/client.py b/example/client.py
new file mode 100755
index 0000000..f23313e
--- /dev/null
+++ b/example/client.py
@@ -0,0 +1,28 @@
+#!/usr/bin/env python3
+import argparse
+import cv_bridge
+import cv2
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from detic_ros.srv import DeticSeg
+import matplotlib.pyplot as plt
+
+if __name__ == '__main__':
+ parser = argparse.ArgumentParser()
+ parser.add_argument('-image', type=str, help='input image path')
+ args = parser.parse_args()
+ image = cv2.imread(args.image)
+ assert image is not None
+
+ rospy.init_node('example_client')
+ rospy.wait_for_service('/docker/detic_segmentor/segment_image')
+ image_msg = CvBridge().cv2_to_imgmsg(image, encoding='passthrough')
+
+ f = rospy.ServiceProxy('/docker/detic_segmentor/segment_image', DeticSeg)
+ resp = f(image_msg)
+ seg_info = resp.seg_info
+ if resp.debug_image is not None:
+ image = CvBridge().imgmsg_to_cv2(resp.debug_image)
+ plt.imshow(image)
+ plt.show()
diff --git a/launch/sample.launch b/launch/sample.launch
index 0000d9b..14de5c6 100644
--- a/launch/sample.launch
+++ b/launch/sample.launch
@@ -2,6 +2,7 @@
+
@@ -26,6 +27,7 @@
output="screen" >
+
diff --git a/node_script/node.py b/node_script/node.py
index eaac723..0f028a2 100755
--- a/node_script/node.py
+++ b/node_script/node.py
@@ -5,6 +5,7 @@
from rospy import Subscriber, Publisher
from sensor_msgs.msg import Image
from detic_ros.msg import SegmentationInfo
+from detic_ros.srv import DeticSeg, DeticSegRequest, DeticSegResponse
from node_config import NodeConfig
from wrapper import DeticWrapper
@@ -25,13 +26,17 @@ def __init__(self, node_config: Optional[NodeConfig]=None):
# As for large buff_size please see:
# https://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/?answer=220505?answer=220505#post-id-22050://answers.ros.org/question/220502/image-subscriber-lag-despite-queue-1/?answer=220505?answer=220505#post-id-220505
- self.sub = rospy.Subscriber('~input_image', Image, self.callback, queue_size=1, buff_size=2**24)
- self.pub_debug_image = rospy.Publisher('~debug_image', Image, queue_size=1)
- self.pub_debug_segmentation_image = rospy.Publisher('~debug_segmentation_image', Image, queue_size=10)
- self.pub_info = rospy.Publisher('~segmentation_info', SegmentationInfo, queue_size=1)
+ self.srv_handler = rospy.Service('~segment_image', DeticSeg, self.callback_srv)
+
+ if node_config.enable_pubsub:
+ self.sub = rospy.Subscriber('~input_image', Image, self.callback_image, queue_size=1, buff_size=2**24)
+ self.pub_debug_image = rospy.Publisher('~debug_image', Image, queue_size=1)
+ self.pub_debug_segmentation_image = rospy.Publisher('~debug_segmentation_image', Image, queue_size=10)
+ self.pub_info = rospy.Publisher('~segmentation_info', SegmentationInfo, queue_size=1)
+
rospy.loginfo('initialized node')
- def callback(self, msg: Image):
+ def callback_image(self, msg: Image):
seginfo, debug_img, debug_seg_img = self.detic_wrapper.infer(msg)
self.pub_info.publish(seginfo)
if debug_img is not None:
@@ -39,6 +44,16 @@ def callback(self, msg: Image):
if debug_seg_img is not None:
self.pub_debug_segmentation_image.publish(debug_seg_img)
+ def callback_srv(self, req: DeticSegRequest) -> DeticSegResponse:
+ msg = req.image
+ seginfo, debug_img, _ = self.detic_wrapper.infer(msg)
+
+ resp = DeticSegResponse()
+ resp.seg_info = seginfo
+ if debug_img is not None:
+ resp.debug_image = debug_img
+ return resp
+
if __name__=='__main__':
rospy.init_node('detic_node', anonymous=True)
diff --git a/node_script/node_config.py b/node_script/node_config.py
index 6c636e3..434613a 100644
--- a/node_script/node_config.py
+++ b/node_script/node_config.py
@@ -16,6 +16,7 @@
@dataclass
class NodeConfig:
+ enable_pubsub: bool
out_debug_img: bool
out_debug_segimage: bool
verbose: bool
@@ -28,6 +29,7 @@ class NodeConfig:
@classmethod
def from_args(cls,
+ enable_pubsub: bool = True,
out_debug_img: bool = True,
out_debug_segimage: bool = True,
verbose: bool = False,
@@ -53,6 +55,7 @@ def from_args(cls,
'Detic_LCOCOI21k_CLIP_SwinB_896b32_4x_ft4x_max-size.pth')
return cls(
+ enable_pubsub,
out_debug_img,
out_debug_segimage,
verbose,
@@ -67,6 +70,7 @@ def from_args(cls,
def from_rosparam(cls):
return cls.from_args(
+ rospy.get_param('~enable_pubsub', True),
rospy.get_param('~out_debug_img', True),
rospy.get_param('~out_debug_segimage', False),
rospy.get_param('~verbose', True),
diff --git a/srv/DeticSeg.srv b/srv/DeticSeg.srv
new file mode 100644
index 0000000..8b0d429
--- /dev/null
+++ b/srv/DeticSeg.srv
@@ -0,0 +1,4 @@
+sensor_msgs/Image image
+---
+detic_ros/SegmentationInfo seg_info
+sensor_msgs/Image debug_image