[jsk_image_capture] add jsk_image_capture pacakge

jsk_image_capture/CMakeLists.txt

@@ -0,0 +1,24 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(jsk_image_capture)
+
+find_package(catkin REQUIRED COMPONENTS
+    roscpp
+    rospy
+    message_generation
+)
+
+catkin_python_setup()
+
+add_service_files(
+  FILES
+  Capture.srv
+)
+
+generate_messages(
+  DEPENDENCIES
+)
+
+catkin_package(
+  CATKIN_DEPENDS
+  message_runtime
+)

jsk_image_capture/euslisp/jsk-image-capture-utils.l

@@ -0,0 +1,11 @@
+(ros::load-ros-manifest "jsk_image_capture")
+
+(defparameter *jsk-image-capture-service-name* "/image_capture_server/capture")
+
+(defun capture-image (image-topic file-name)
+  (let (res (req (instance jsk_image_capture::CaptureRequest :init)))
+    (send req :image_topic image-topic)
+    (send req :file_name file-name)
+    (setq res (ros::service-call *jsk-image-capture-service-name* req t))
+    (send res :success)
+    ))

jsk_image_capture/launch/demo.launch

@@ -0,0 +1,21 @@
+<launch>
+  <arg name="video_height" default="480" />
+  <arg name="video_width" default="640" />
+  <arg name="video_framerate" default="30" />
+  <node name="usb_cam_node" pkg="usb_cam" type="usb_cam_node">
+    <rosparam subst_value="true">
+      image_height: $(arg video_height)
+      image_width: $(arg video_width)
+      framerate: $(arg video_framerate)
+      pixel_format: yuyv
+    </rosparam>
+  </node>
+
+  <node name="image_capture_server" pkg="jsk_image_capture" type="image_capture_server" output="screen"/>
+
+  <node name="sample_image_capture_client" pkg="jsk_image_capture" type="sample_image_capture_client.py" output="screen" >
+      <rosparam>
+          image_topic: /usb_cam_node/image_raw
+      </rosparam>
+  </node>
+</launch>

jsk_image_capture/node_scripts/image_capture

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from jsk_image_capture.jsk_image_capture import capture_image
+import sys
+import argparse
+from cv_bridge import CvBridge
+
+
+def main():
+
+    myargv = rospy.myargv(argv=sys.argv)
+    parser = argparse.ArgumentParser()
+    parser.add_argument('image_topic', help='Image topic to save an image.')
+    parser.add_argument('file_name', help='Filename of saved image.')
+    args = parser.parse_args(args=myargv)
+
+    rospy.init_node('image_capture', anonymous=True)
+    cv_bridge = CvBridge()
+    success, message = capture_image(
+                            args.image_topic,
+                            args.file_name,
+                            cv_bridge
+                            )
+
+    if success:
+        rospy.loginfo('Successfully saved image from {} to {}.'.format(
+                            args.image_topic,
+                            args.file_name
+                            ))
+    else:
+        rospy.logerr('Failed to save image from {} to {}.'.format(
+                            args.image_topic,
+                            args.file_name
+                            ))
+
+
+if __name__ == '__main__':
+    main()

jsk_image_capture/node_scripts/image_capture_server

@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from jsk_image_capture.jsk_image_capture import ImageCaptureServer
+
+
+def main():
+
+    rospy.init_node('image_capture_server')
+    server = ImageCaptureServer()
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    main()

jsk_image_capture/node_scripts/sample_image_capture_client.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from jsk_image_capture.jsk_image_capture import ImageCaptureClient
+
+
+def main():
+
+    rospy.init_node('sample_image_capture_client')
+    image_topic = rospy.get_param('~image_topic')
+    directory = rospy.get_param('~directory', '/tmp')
+    client = ImageCaptureClient()
+    rate = rospy.Rate(1)
+    for index in range(10):
+        rate.sleep()
+        file_name = directory + '/sample_image_{}.jpg'.format(index)
+        client.capture(image_topic, file_name)
+        rospy.loginfo('Capturing image from {} to {}'.format(image_topic, file_name))
+
+
+if __name__ == '__main__':
+    main()

jsk_image_capture/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>jsk_image_capture</name>
+  <version>2.2.11</version>
+  <description>The jsk_image_capture package</description>
+
+  <maintainer email="[email protected]">Koki Shinjo</maintainer>
+  <author email="[email protected]">Koki Shinjo</author>
+
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+  <export>
+  </export>
+</package>

jsk_image_capture/setup.py

@@ -0,0 +1,9 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['jsk_image_capture'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

jsk_image_capture/src/jsk_image_capture/jsk_image_capture.py

@@ -0,0 +1,69 @@
+# -*- encoding: utf-8 -*-
+
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+import cv2
+
+from .srv import Capture, CaptureRequest, CaptureResponse
+
+
+def capture_image(image_topic, file_name, cv_bridge):
+    duration_timeout = rospy.get_param('~duration_timeout', 10.0)
+    try:
+        msg = rospy.wait_for_message(
+                        image_topic,
+                        Image,
+                        timeout=rospy.Duration(duration_timeout)
+                        )
+        image = cv_bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
+        cv2.imwrite(file_name, image)
+        return True, 'Success'
+    except rospy.ROSException as e:
+        rospy.loginfo('Error: {]'.format(e))
+        return False, 'Error: {}'.format(e)
+
+
+class ImageCaptureServer:
+
+    def __init__(self):
+        self.cv_bridge = CvBridge()
+        self.srv = rospy.Service(
+                        '~capture',
+                        Capture,
+                        self.handler)
+
+    def handler(self, req):
+        success, message = capture_image(
+                                req.image_topic,
+                                req.file_name,
+                                self.cv_bridge
+                                )
+        res = CaptureResponse()
+        res.success = success
+        res.message = message
+        return res
+
+
+class ImageCaptureClient:
+
+    def __init__(self):
+
+        rospy.wait_for_service(
+                '/image_capture_server/capture',
+                rospy.Duration(10)
+                )
+        self.client = rospy.ServiceProxy(
+                '/image_capture_server/capture',
+                Capture
+                )
+
+    def capture(self,
+                image_topic,
+                file_name
+                ):
+        req = CaptureRequest()
+        req.image_topic = image_topic
+        req.file_name = file_name
+        ret = self.client(req)
+        return ret.success, ret.message

jsk_image_capture/srv/Capture.srv

@@ -0,0 +1,5 @@
+string image_topic
+string file_name
+---
+bool success
+string message