[jsk_image_capture] add jsk_image_capture pacakge

jsk_ros_patch/image_view2/CMakeLists.txt

@@ -19,6 +19,14 @@ endif()
 
 find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure  cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters message_generation std_srvs pcl_ros)
 
+find_package(PCL REQUIRED)
+
+find_package(OpenCV REQUIRED)
+
+find_package(Boost REQUIRED COMPONENTS)
+
+catkin_python_setup()
+
 generate_dynamic_reconfigure_options(
   cfg/ImageView2.cfg
 )
@@ -28,34 +36,34 @@ add_message_files(
 )
 
 add_service_files(
-  FILES ChangeMode.srv
+  FILES ChangeMode.srv Capture.srv
   )
 
-include_directories(include ${catkin_INCLUDE_DIRS})
+generate_messages(DEPENDENCIES geometry_msgs std_msgs)
+
+catkin_package(
+    DEPENDS OpenCV PCL
+    CATKIN_DEPENDS roscpp cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters message_runtime
+    INCLUDE_DIRS include
+)
+
+include_directories(
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${PCL_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIRS}
+)
 
 # Image viewers
-find_package(OpenCV REQUIRED)
-add_executable(image_view2 image_view2.cpp image_view2_node.cpp)
+add_executable(image_view2 src/image_view2.cpp src/image_view2_node.cpp)
 target_link_libraries(image_view2 ${OpenCV_LIBS} ${catkin_LIBRARIES})
 add_dependencies(image_view2 ${PROJECT_NAME}_gencpp ${PROJECT_NAME}_gencfg)
 
 # Point Rectangle Extractor
-add_executable(points_rectangle_extractor points_rectangle_extractor.cpp)
-find_package(Boost REQUIRED COMPONENTS)
-include_directories(${Boost_INCLUDE_DIRS})
-find_package(PCL REQUIRED)
-include_directories(${PCL_INCLUDE_DIRS})
+add_executable(points_rectangle_extractor src/points_rectangle_extractor.cpp)
 target_link_libraries(points_rectangle_extractor ${Boost_LIBRARIES} ${catkin_LIBRARIES})
 add_dependencies(points_rectangle_extractor ${PROJECT_NAME}_gencpp)
 
-generate_messages(DEPENDENCIES geometry_msgs std_msgs)
-
-catkin_package(
-    DEPENDS OpenCV PCL
-    CATKIN_DEPENDS roscpp cv_bridge std_msgs sensor_msgs geometry_msgs image_transport tf image_geometry message_filters
-    INCLUDE_DIRS # TODO include
-    LIBRARIES # TODO
-)
 
 # From https://github.com/jsk-ros-pkg/jsk_recognition/pull/2345
 # Install header files directly into ${CATKIN_PACKAGE_INCLUDE_DESTINATION}.
@@ -76,6 +84,18 @@ install(TARGETS image_view2 points_rectangle_extractor
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
+catkin_install_python(
+    PROGRAMS node_scripts/image_capture node_scripts/image_capture_server node_scripts/sample_image_capture_client.py
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY euslisp
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    USE_SOURCE_PERMISSIONS
+)
+install(DIRECTORY sample
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+    USE_SOURCE_PERMISSIONS
+)
 
 if(CATKIN_ENABLE_TESTING)
   find_package(rostest REQUIRED)

jsk_ros_patch/image_view2/euslisp/image-capture-utils.l

@@ -0,0 +1,12 @@
+(ros::load-ros-manifest "image_view2")
+
+(defun capture-image (image-topic file-name)
+  (let (res (req (instance image_view2::CaptureRequest :init)))
+    (send req :image_topic image-topic)
+    (send req :file_name file-name)
+    (setq res (ros::service-call "/image_capture_server/capture" req t))
+    (if (send res :success)
+        (ros::ros-info "Succeed to capture image from ~A to ~A" image-topic file-name)
+      (ros::ros-error "Failed to capture image from ~A to ~A : ~A" image-topic file-name (send res :message)))
+    (send res :success)
+    ))

jsk_ros_patch/image_view2/node_scripts/image_capture

@@ -0,0 +1,40 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from image_view2.image_capture_utils 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_ros_patch/image_view2/node_scripts/image_capture_server

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

jsk_ros_patch/image_view2/node_scripts/sample_image_capture_client.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+# -*- encoding: utf-8 -*-
+
+import rospy
+from image_view2.image_capture_utils 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_ros_patch/image_view2/package.xml

@@ -22,6 +22,7 @@
   <build_depend>message_generation</build_depend>
   <build_depend>pcl_ros</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>rostest</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>std_msgs</build_depend>
@@ -37,10 +38,12 @@
   <exec_depend>message_runtime</exec_depend>
   <exec_depend>pcl_ros</exec_depend>
   <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
   <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>std_srvs</exec_depend>
   <exec_depend>tf</exec_depend>
+  <exec_depend>usb_cam</exec_depend>
 
   <test_depend condition="$ROS_PYTHON_VERSION == 2">python-numpy</test_depend>
   <test_depend condition="$ROS_PYTHON_VERSION == 3">python3-numpy</test_depend>

jsk_ros_patch/image_view2/sample/sample_image_capture.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="image_view2" type="image_capture_server" output="screen"/>
+
+  <node name="sample_image_capture_client" pkg="image_view2" type="sample_image_capture_client.py" output="screen" >
+      <rosparam>
+          image_topic: /usb_cam_node/image_raw
+      </rosparam>
+  </node>
+</launch>

jsk_ros_patch/image_view2/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=['image_view2'],
+    package_dir={'': 'src'}
+)
+
+setup(**d)

jsk_ros_patch/image_view2/src/image_view2/image_capture_utils.py

@@ -0,0 +1,83 @@
+# -*- encoding: utf-8 -*-
+
+import rospy
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+import cv2
+
+from image_view2.srv import Capture
+from image_view2.srv import CaptureRequest
+from image_view2.srv import CaptureResponse
+
+from queue import Queue
+
+
+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.task_queue = Queue()
+        self.srv = rospy.Service(
+                        '~capture',
+                        Capture,
+                        self.handler)
+
+    def spin(self):
+
+        rate = rospy.Rate(1)
+        while not rospy.is_shutdown():
+            rate.sleep()
+            if not self.task_queue.empty():
+                req = self.task_queue.get()
+                success, message = capture_image(
+                                req.image_topic,
+                                req.file_name,
+                                self.cv_bridge
+                                )
+                rospy.loginfo('Capture a image: {}, {}'.format(success, message))
+
+    def handler(self, req):
+        self.task_queue.put(req)
+        res = CaptureResponse()
+        res.success = True
+        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_ros_patch/image_view2/srv/Capture.srv

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