diff --git a/jsk_ros_patch/image_view2/CMakeLists.txt b/jsk_ros_patch/image_view2/CMakeLists.txt
index c9509c3c9..91563d3ed 100644
--- a/jsk_ros_patch/image_view2/CMakeLists.txt
+++ b/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)
diff --git a/jsk_ros_patch/image_view2/euslisp/image-capture-utils.l b/jsk_ros_patch/image_view2/euslisp/image-capture-utils.l
new file mode 100644
index 000000000..7c0a5972c
--- /dev/null
+++ b/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)
+ ))
diff --git a/jsk_ros_patch/image_view2/image_view2.h b/jsk_ros_patch/image_view2/include/image_view2.h
similarity index 100%
rename from jsk_ros_patch/image_view2/image_view2.h
rename to jsk_ros_patch/image_view2/include/image_view2.h
diff --git a/jsk_ros_patch/image_view2/node_scripts/image_capture b/jsk_ros_patch/image_view2/node_scripts/image_capture
new file mode 100755
index 000000000..fb2c48bbc
--- /dev/null
+++ b/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()
diff --git a/jsk_ros_patch/image_view2/node_scripts/image_capture_server b/jsk_ros_patch/image_view2/node_scripts/image_capture_server
new file mode 100755
index 000000000..17a4bee8f
--- /dev/null
+++ b/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()
diff --git a/jsk_ros_patch/image_view2/node_scripts/sample_image_capture_client.py b/jsk_ros_patch/image_view2/node_scripts/sample_image_capture_client.py
new file mode 100755
index 000000000..0bde17d01
--- /dev/null
+++ b/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()
diff --git a/jsk_ros_patch/image_view2/package.xml b/jsk_ros_patch/image_view2/package.xml
index 0c39fe811..1c8b369a3 100644
--- a/jsk_ros_patch/image_view2/package.xml
+++ b/jsk_ros_patch/image_view2/package.xml
@@ -22,6 +22,7 @@
message_generation
pcl_ros
roscpp
+ rospy
rostest
sensor_msgs
std_msgs
@@ -37,10 +38,12 @@
message_runtime
pcl_ros
roscpp
+ rospy
sensor_msgs
std_msgs
std_srvs
tf
+ usb_cam
python-numpy
python3-numpy
diff --git a/jsk_ros_patch/image_view2/sample/sample_image_capture.launch b/jsk_ros_patch/image_view2/sample/sample_image_capture.launch
new file mode 100644
index 000000000..d9dce86b8
--- /dev/null
+++ b/jsk_ros_patch/image_view2/sample/sample_image_capture.launch
@@ -0,0 +1,21 @@
+
+
+
+
+
+
+ image_height: $(arg video_height)
+ image_width: $(arg video_width)
+ framerate: $(arg video_framerate)
+ pixel_format: yuyv
+
+
+
+
+
+
+
+ image_topic: /usb_cam_node/image_raw
+
+
+
diff --git a/jsk_ros_patch/image_view2/setup.py b/jsk_ros_patch/image_view2/setup.py
new file mode 100644
index 000000000..8e0740340
--- /dev/null
+++ b/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)
diff --git a/jsk_ros_patch/image_view2/image_view2.cpp b/jsk_ros_patch/image_view2/src/image_view2.cpp
similarity index 100%
rename from jsk_ros_patch/image_view2/image_view2.cpp
rename to jsk_ros_patch/image_view2/src/image_view2.cpp
diff --git a/jsk_ros_patch/image_view2/src/image_view2/__init__.py b/jsk_ros_patch/image_view2/src/image_view2/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/jsk_ros_patch/image_view2/src/image_view2/image_capture_utils.py b/jsk_ros_patch/image_view2/src/image_view2/image_capture_utils.py
new file mode 100644
index 000000000..e6da9ef6d
--- /dev/null
+++ b/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
diff --git a/jsk_ros_patch/image_view2/image_view2_node.cpp b/jsk_ros_patch/image_view2/src/image_view2_node.cpp
similarity index 100%
rename from jsk_ros_patch/image_view2/image_view2_node.cpp
rename to jsk_ros_patch/image_view2/src/image_view2_node.cpp
diff --git a/jsk_ros_patch/image_view2/points_rectangle_extractor.cpp b/jsk_ros_patch/image_view2/src/points_rectangle_extractor.cpp
similarity index 100%
rename from jsk_ros_patch/image_view2/points_rectangle_extractor.cpp
rename to jsk_ros_patch/image_view2/src/points_rectangle_extractor.cpp
diff --git a/jsk_ros_patch/image_view2/srv/Capture.srv b/jsk_ros_patch/image_view2/srv/Capture.srv
new file mode 100644
index 000000000..166a7e517
--- /dev/null
+++ b/jsk_ros_patch/image_view2/srv/Capture.srv
@@ -0,0 +1,5 @@
+string image_topic
+string file_name
+---
+bool success
+string message