diff --git a/jsk_common/package.xml b/jsk_common/package.xml
index 326fa7e55..2a643899e 100644
--- a/jsk_common/package.xml
+++ b/jsk_common/package.xml
@@ -19,6 +19,7 @@
audio_video_recorder
dynamic_tf_publisher
image_view2
+ jsk_rosbag_tools
jsk_topic_tools
jsk_tools
multi_map_server
diff --git a/jsk_rosbag_tools/.gitignore b/jsk_rosbag_tools/.gitignore
new file mode 100644
index 000000000..571491802
--- /dev/null
+++ b/jsk_rosbag_tools/.gitignore
@@ -0,0 +1,2 @@
+!.gitignore
+requirements.txt
diff --git a/jsk_rosbag_tools/CMakeLists.txt b/jsk_rosbag_tools/CMakeLists.txt
new file mode 100644
index 000000000..307da0895
--- /dev/null
+++ b/jsk_rosbag_tools/CMakeLists.txt
@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_rosbag_tools)
+
+find_package(
+ catkin REQUIRED
+ catkin_virtualenv
+)
+
+if (${catkin_virtualenv_VERSION} VERSION_LESS "0.5.1")
+ message(STATUS "jsk_rosbag_tools requires catkin_virtualenv >= 0.5.1")
+ return()
+endif()
+
+
+catkin_python_setup()
+
+catkin_package(
+ CATKIN_DEPENDS
+)
+
+if($ENV{ROS_DISTRO} STREQUAL "noetic")
+ catkin_generate_virtualenv(
+ INPUT_REQUIREMENTS requirements.in
+ PYTHON_INTERPRETER python3
+ )
+else()
+ catkin_generate_virtualenv(
+ INPUT_REQUIREMENTS requirements.in
+ )
+endif()
+
+file(GLOB SCRIPTS_FILES scripts/*)
+catkin_install_python(
+ PROGRAMS ${SCRIPTS_FILES}
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+catkin_download(download_audio_data
+ https://drive.google.com/uc?export=download&id=1rFZYoFjLqIWjEe0DaNiL3k9893m31nu7
+ DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data
+ FILENAME 2022-05-07-hello-test.bag
+ MD5 3650e27dad2c7dc0e447033259290db6
+)
+catkin_download(download_video_data
+ https://drive.google.com/uc?export=download&id=1v4YNOHnHYxLOty1lYR2R6lfNF0itCwK7
+ DESTINATION ${CMAKE_CURRENT_SOURCE_DIR}/samples/data
+ FILENAME 20220530173950_go_to_kitchen_rosbag.bag
+ MD5 d51fa8aeacd36f7aaa1597b67bd9ffdf
+)
+add_custom_target(download ALL DEPENDS download_audio_data download_video_data)
+
+if(CATKIN_ENABLE_TESTING)
+ find_package(catkin REQUIRED COMPONENTS roslint rostest)
+
+ set(python_test_scripts
+ tests/test_jsk_rosbag_tools.py
+ tests/test_bag_to_video.py
+ )
+
+ roslint_python()
+ roslint_python(${python_test_scripts})
+ roslint_add_test()
+
+ catkin_install_python(PROGRAMS ${python_test_scripts}
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+ add_rostest(tests/test_jsk_rosbag_tools.test
+ DEPENDENCIES ${PROJECT_NAME}_generate_virtualenv download_audio_data download_video_data)
+ if("$ENV{ROS_DISTRO}" STRGREATER "indigo")
+ # could not install moviepy on indigo.
+ add_rostest(tests/test_bag_to_video.test
+ DEPENDENCIES ${PROJECT_NAME}_generate_virtualenv download_audio_data download_video_data)
+ endif()
+endif()
+
+install(DIRECTORY scripts samples
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ USE_SOURCE_PERMISSIONS
+)
+
+install(FILES requirements.txt
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/jsk_rosbag_tools/README.md b/jsk_rosbag_tools/README.md
new file mode 100644
index 000000000..eb97d296b
--- /dev/null
+++ b/jsk_rosbag_tools/README.md
@@ -0,0 +1,195 @@
+# jsk_rosbag_tools
+
+Tools such as creating video from rosbag and compressing rosbag images.
+
+## bag_to_video.py
+
+Create video from rosbag.
+
+### Usage
+
+```
+usage: bag_to_video.py [-h] [--out OUT] [--fps FPS] [--samplerate SAMPLERATE] [--channels CHANNELS] [--audio-topic AUDIO_TOPIC] [--image-topic IMAGE_TOPIC [IMAGE_TOPIC ...]] input_bagfile
+
+rosbag to video
+
+positional arguments:
+ input_bagfile
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out OUT, -o OUT output directory path or filename.
+ If more than one --image-topic are specified,
+ this will be interpreted as a directory name.
+ Otherwise this is the file name.
+ --fps FPS
+ --samplerate SAMPLERATE, -r SAMPLERATE
+ sampling rate
+ --channels CHANNELS number of input channels
+ --audio-topic AUDIO_TOPIC
+ --image-topic IMAGE_TOPIC [IMAGE_TOPIC ...]
+ Topic name to extract.
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools bag_to_video.py $(rospack find jsk_rosbag_tools)/samples/data/20220530173950_go_to_kitchen_rosbag.bag \
+ --samplerate 16000 --channels 1 --audio-topic /audio \
+ --image-topic /head_camera/rgb/throttled/image_rect_color/compressed \
+ -o /tmp/20220530173950_go_to_kitchen_rosbag.mp4
+```
+
+## bag_to_audio.py
+
+Create audio file from rosbag.
+
+### Usage
+
+```
+usage: bag_to_audio.py [-h] [--out OUT] [--samplerate SAMPLERATE] [--channels CHANNELS] [--audio-topic AUDIO_TOPIC] input_bagfile
+
+rosbag to audio
+
+positional arguments:
+ input_bagfile
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out OUT, -o OUT output filename. If `--audio-topic`_info is exists, you don't have to specify samplerate and channels.
+ --samplerate SAMPLERATE, -r SAMPLERATE
+ sampling rate
+ --channels CHANNELS number of input channels
+ --audio-topic AUDIO_TOPIC
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools bag_to_audio.py $(rospack find jsk_rosbag_tools)/samples/data/20220530173950_go_to_kitchen_rosbag.bag \
+ --samplerate 16000 --channels 1 --audio-topic /audio \
+ -o /tmp/20220530173950_go_to_kitchen_rosbag.wav
+```
+
+## video_to_bag.py
+
+Convert video file to bagfile.
+
+### Usage
+
+```
+usage: video_to_bag.py [-h] [--out output_file] [--topic-name TOPIC_NAME] [--compress] [--no-progress-bar] inputvideo
+
+Convert video to bag.
+
+positional arguments:
+ inputvideo
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out output_file, -o output_file
+ name of the output bag file
+ --topic-name TOPIC_NAME
+ Converted topic name.
+ --compress Compress Image flag.
+ --no-progress-bar Don't show progress bar.
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools video_to_bag.py /tmp/output_bag/head_camera--slash--rgb--slash--throttled--slash--image_rect_color--slash--compressed-with-audio.mp4 \
+ -o /tmp/output_bag/video.bag --compress
+```
+
+## compress_imgs.py
+
+Convert `Image` messages to `CompressedImage` or `CompressedDepthImage`.
+
+### Usage
+
+```
+usage: compress_imgs.py [-h] [--out OUT] [--compressed-topics [COMPRESSED_TOPICS [COMPRESSED_TOPICS ...]]] [--replace] [--no-progress-bar] input_bagfile
+
+Convert Image messages to CompressedImage or CompressedDepthImage
+
+positional arguments:
+ input_bagfile input bagfile path
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out OUT, -o OUT output bagfile path
+ --compressed-topics [COMPRESSED_TOPICS [COMPRESSED_TOPICS ...]]
+ this image topics are compressed
+ --replace
+ --no-progress-bar Don't show progress bar.
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools compress_imgs.py $(rospack find jsk_rosbag_tools)/samples/data/20220530173950_go_to_kitchen_rosbag.bag \
+ -o /tmp/20220530173950_go_to_kitchen_rosbag-compressed.bag
+```
+
+## tf_static_to_tf.py
+
+Convert tf_static to tf and save it as a rosbag.
+
+```
+usage: tf_static_to_tf.py [-h] [--out OUT] [--no-progress-bar] input_bagfile
+
+Convert tf_static to tf and save it as a rosbag
+
+positional arguments:
+ input_bagfile input bagfile path
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out OUT, -o OUT output bagfile path
+ --no-progress-bar Don't show progress bar.
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools tf_static_to_tf.py $(rospack find jsk_rosbag_tools)/samples/data/20220530173950_go_to_kitchen_rosbag.bag
+```
+
+### Note
+
+`jsk_topic_tools` has a [static_tf_republisher.py](https://jsk-docs.readthedocs.io/projects/jsk_common/en/latest/jsk_topic_tools/scripts/static_tf_republisher.html) which republish `/tf_static` from a rosbag file.
+
+`tf_static_to_tf.py` is an approach to rewrite the rosbag file.
+
+## merge.py
+
+Merges two bagfiles.
+
+### Usage
+
+```
+usage: merge.py [-h] [--out output_file] [--topics TOPICS] [-i] main_bagfile bagfile
+
+Merges two bagfiles.
+
+positional arguments:
+ main_bagfile path to a bagfile, which will be the main bagfile
+ bagfile path to a bagfile which should be merged to the main bagfile
+
+optional arguments:
+ -h, --help show this help message and exit
+ --out output_file, -o output_file
+ name of the output file
+ --topics TOPICS, -t TOPICS
+ topics which should be merged to the main bag
+ -i reindex bagfile
+```
+
+### Example
+
+```
+rosrun jsk_rosbag_tools merge.py \
+ $(rospack find jsk_rosbag_tools)/samples/data/20220530173950_go_to_kitchen_rosbag.bag \
+ $(rospack find jsk_rosbag_tools)/samples/data/2022-05-07-hello-test.bag
+```
diff --git a/jsk_rosbag_tools/package.xml b/jsk_rosbag_tools/package.xml
new file mode 100644
index 000000000..87d3eef03
--- /dev/null
+++ b/jsk_rosbag_tools/package.xml
@@ -0,0 +1,42 @@
+
+
+ jsk_rosbag_tools
+ 2.2.11
+ The rosbag tools
+ BSD
+
+ Iori Yanokura
+ Iori Yanokura
+
+ catkin
+ python-catkin-pkg-modules
+
+ catkin_virtualenv
+
+ audio_common_msgs
+ cv_bridge
+ ffmpeg
+ python-numpy
+ python-rospkg-modules
+ python-scipy
+ python-setuptools
+ python-termcolor
+ python-tqdm
+ python-yaml
+ python3-numpy
+ python3-rospkg-modules
+ python3-scipy
+ python3-setuptools
+ python3-termcolor
+ python3-tqdm
+ python3-yaml
+ sensor_msgs
+
+ roslint
+ rostest
+
+
+ requirements.txt
+
+
+
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/__init__.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/__init__.py
new file mode 100644
index 000000000..9c0fa90a1
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/__init__.py
@@ -0,0 +1 @@
+# flake8: noqa
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_audio.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_audio.py
new file mode 100644
index 000000000..acdab61f8
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_audio.py
@@ -0,0 +1,50 @@
+import os
+import os.path as osp
+
+import numpy as np
+import rosbag
+from scipy.io.wavfile import write as wav_write
+
+from jsk_rosbag_tools.extract import extract_oneshot_topic
+from jsk_rosbag_tools.info import get_topic_dict
+from jsk_rosbag_tools.makedirs import makedirs
+
+
+def bag_to_audio(bag_filepath,
+ wav_outpath,
+ topic_name='/audio',
+ audio_info_topic_name=None,
+ samplerate=44100,
+ channels=1,
+ overwrite=True):
+ if os.path.exists(wav_outpath) and overwrite is False:
+ raise FileExistsError('{} file already exists.'.format(wav_outpath))
+ topic_dict = get_topic_dict(bag_filepath)
+ if topic_name not in topic_dict:
+ return
+
+ audio_info_topic_name = audio_info_topic_name or topic_name + '_info'
+ # if audio_info_topic exists, extract info from it.
+ if audio_info_topic_name in topic_dict:
+ audio_info = extract_oneshot_topic(bag_filepath, audio_info_topic_name)
+ if audio_info is not None:
+ samplerate = audio_info.sample_rate
+ channels = audio_info.channels
+
+ bag = rosbag.Bag(bag_filepath)
+ audio_buffer = []
+ for _, msg, _ in bag.read_messages(topics=[topic_name]):
+ if msg._type == 'audio_common_msgs/AudioData':
+ buf = np.frombuffer(msg.data, dtype='int16')
+ buf = buf.reshape(-1, channels)
+ audio_buffer.append(buf)
+ audio_buffer = np.concatenate(audio_buffer, axis=0)
+
+ makedirs(osp.dirname(wav_outpath))
+ wav_write(wav_outpath, rate=samplerate, data=audio_buffer)
+
+ valid = os.stat(wav_outpath).st_size != 0
+ if valid is False:
+ return False
+
+ return True
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py
new file mode 100644
index 000000000..ce99028c7
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/bag_to_video.py
@@ -0,0 +1,161 @@
+import datetime
+import os
+import os.path as osp
+import sys
+import tempfile
+
+import cv2
+from moviepy.audio.io.AudioFileClip import AudioFileClip
+from moviepy.video.io.ffmpeg_writer import FFMPEG_VideoWriter
+from moviepy.video.io.VideoFileClip import VideoFileClip
+from tqdm import tqdm
+
+from jsk_rosbag_tools.bag_to_audio import bag_to_audio
+from jsk_rosbag_tools.extract import extract_image_topic
+from jsk_rosbag_tools.extract import get_image_topic_names
+from jsk_rosbag_tools.image_utils import \
+ resize_keeping_aspect_ratio_wrt_target_size
+from jsk_rosbag_tools.info import get_topic_dict
+from jsk_rosbag_tools.makedirs import makedirs
+from jsk_rosbag_tools.topic_name_utils import topic_name_to_file_name
+
+
+def bag_to_video(input_bagfile,
+ output_filepath=None,
+ output_dirpath=None,
+ image_topic=None,
+ image_topics=None,
+ fps=30,
+ samplerate=16000,
+ channels=1,
+ audio_topic='/audio',
+ show_progress_bar=True):
+ """Create video from rosbag file.
+
+ Specify only either output_filepath or output_dirpath.
+ If output_filepath is specified, specify image_topic.
+ If output_dirpath is specified, image_topics can be specified.
+ If image_topic_names is None, make all color images into video.
+
+ """
+ if not os.path.exists(input_bagfile):
+ print('[bag_to_video] Input bagfile {} not exists.'
+ .format(input_bagfile))
+ sys.exit(1)
+
+ if output_filepath is not None and output_dirpath is not None:
+ raise ValueError(
+ 'Specify only either output_filepath or output_dirpath.')
+
+ output_filepaths = []
+ target_image_topics = []
+ candidate_topics = get_image_topic_names(input_bagfile)
+
+ if output_filepath is not None:
+ if image_topic is None:
+ raise ValueError(
+ 'If output_filepath is specified, specify image_topic.')
+ output_filepaths.append(output_filepath)
+ target_image_topics.append(image_topic)
+ wav_outpath = tempfile.NamedTemporaryFile(suffix='.wav').name
+ else:
+ # output_dirpath is specified case.
+ if image_topics is None:
+ # record all color topics
+ image_topics = get_image_topic_names(input_bagfile,
+ rgb_only=True)
+ target_image_topics = image_topics
+
+ for image_topic in target_image_topics:
+ output_filepaths.append(
+ osp.join(
+ output_dirpath,
+ topic_name_to_file_name(image_topic) + '.mp4'))
+ wav_outpath = osp.join(output_dirpath, '{}.wav'.format(
+ topic_name_to_file_name(audio_topic)))
+
+ # check topics exist.
+ not_exists_topics = list(filter(
+ lambda tn: tn not in candidate_topics, target_image_topics))
+ if len(not_exists_topics) > 0:
+ raise ValueError(
+ 'Topics that are not included in the rosbag are specified.'
+ ' {}'.format(list(not_exists_topics)))
+
+ print('[bag_to_video] Extracting audio from rosbag file.')
+ audio_exists = bag_to_audio(input_bagfile, wav_outpath,
+ samplerate=samplerate,
+ channels=channels,
+ topic_name=audio_topic)
+
+ dt = 1.0 / fps
+ for image_topic, output_filepath in zip(target_image_topics,
+ output_filepaths):
+ print('[bag_to_video] Creating video of {} from rosbag file {}.'
+ .format(image_topic, input_bagfile))
+ filepath_dir = osp.dirname(output_filepath)
+ if filepath_dir:
+ makedirs(filepath_dir)
+ if audio_exists:
+ tmp_videopath = tempfile.NamedTemporaryFile(suffix='.mp4').name
+ else:
+ tmp_videopath = output_filepath
+
+ images = extract_image_topic(input_bagfile, image_topic)
+ topic_info_dict = get_topic_dict(input_bagfile)[image_topic]
+ n_frame = topic_info_dict['messages']
+
+ if show_progress_bar:
+ progress = tqdm(total=n_frame)
+
+ # remove 0 time stamp
+ stamp = 0.0
+ while stamp == 0.0:
+ stamp, _, img, _ = next(images)
+ if show_progress_bar:
+ progress.update(1)
+ start_stamp = stamp
+ width, height = img.shape[1], img.shape[0]
+
+ creation_time = datetime.datetime.utcfromtimestamp(start_stamp)
+ time_format = '%y-%m-%d %h:%M:%S'
+ writer = FFMPEG_VideoWriter(
+ tmp_videopath,
+ (width, height),
+ fps, logfile=None,
+ ffmpeg_params=[
+ '-metadata',
+ 'creation_time={}'.format(
+ creation_time.strftime(time_format)),
+ ])
+
+ current_time = 0.0
+ cur_img = resize_keeping_aspect_ratio_wrt_target_size(
+ cv2.cvtColor(img, cv2.COLOR_BGR2RGB), width=width, height=height)
+ for i, (stamp, _, bgr_img, _) in enumerate(images):
+ if show_progress_bar:
+ progress.update(1)
+ aligned_stamp = stamp - start_stamp
+ while current_time < aligned_stamp:
+ current_time += dt
+ writer.write_frame(cur_img)
+ cur_img = resize_keeping_aspect_ratio_wrt_target_size(
+ cv2.cvtColor(bgr_img, cv2.COLOR_BGR2RGB),
+ width=width, height=height)
+ writer.write_frame(cur_img)
+ current_time += dt
+ writer.close()
+
+ if show_progress_bar:
+ progress.close()
+
+ if audio_exists:
+ print('[bag_to_video] Combine video and audio')
+ clip_output = VideoFileClip(tmp_videopath).subclip().\
+ set_audio(AudioFileClip(wav_outpath))
+ clip_output.write_videofile(
+ output_filepath,
+ verbose=False,
+ logger='bar' if show_progress_bar else None)
+ print('[bag_to_video] Created video is saved to {}'
+ .format(output_filepath))
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/compress.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/compress.py
new file mode 100644
index 000000000..eeed738f8
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/compress.py
@@ -0,0 +1,37 @@
+import rosbag
+from tqdm import tqdm
+
+from jsk_rosbag_tools.cv import compress_depth_msg
+from jsk_rosbag_tools.cv import compress_img_msg
+from jsk_rosbag_tools.info import get_topic_dict
+
+
+def compress_bag_imgs(input_bagfilepath, output_bagfilepath,
+ compressed_topics=None,
+ show_progress_bar=True):
+ compressed_topics = compressed_topics or []
+ input_bag = rosbag.Bag(input_bagfilepath)
+
+ topic_dict = get_topic_dict(input_bagfilepath)
+ with rosbag.Bag(output_bagfilepath, 'w') as outbag:
+ if show_progress_bar:
+ progress = tqdm(total=input_bag.get_message_count())
+ for topic, msg, t in input_bag:
+ if show_progress_bar:
+ progress.update(1)
+ # update the progress with a post fix
+ progress.set_postfix(time=t)
+ if topic_dict[topic]['type'] == 'sensor_msgs/Image':
+ if msg.encoding in ['bgra8', 'bgr8',
+ 'rgba8', 'rgb8']:
+ msg = compress_img_msg(msg)
+ topic += '/compressed'
+ elif msg.encoding in ['16UC1', '32FC1']:
+ msg = compress_depth_msg(msg)
+ if topic.lstrip('/') in compressed_topics:
+ topic += '/compressed'
+ else:
+ topic += '/compressedDepth'
+ else:
+ pass
+ outbag.write(topic, msg, t)
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/cv.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/cv.py
new file mode 100644
index 000000000..a408d0e88
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/cv.py
@@ -0,0 +1,232 @@
+import struct
+
+import cv2
+import cv_bridge
+import numpy as np
+import sensor_msgs.msg
+
+
+_bridge = cv_bridge.CvBridge()
+
+
+def img_to_msg(img, encoding="bgr8",
+ compress=True):
+ """Convert numpy image to ROS message.
+
+ """
+ msg = _bridge.cv2_to_imgmsg(img, encoding=encoding)
+ if compress is True:
+ msg = compress_img_msg(msg)
+ return msg
+
+
+def msg_to_img(msg):
+ if msg.encoding in ['bgra8', 'bgr8',
+ 'rgba8', 'rgb8']:
+ return msg_to_bgr(msg)
+ elif msg.encoding in ['mono8']:
+ return msg_to_mono(msg)
+ else:
+ return msg_to_np_depth(msg)
+
+
+def compressed_format(msg):
+ if ';' not in msg.format:
+ fmt = ''
+ if msg.format not in ['png', 'jpeg', 'rvl']:
+ raise RuntimeError(
+ 'Unsupported or invalid compresssion format {}'
+ 'Please report this error'
+ 'https://github.com/jsk-ros-pkg/jsk_common/issues/new'
+ .format(msg.format))
+ if msg.format in ['rvl']:
+ compr_type = 'compressedDepth'
+ else:
+ # could not determine compressed format.
+ compr_type = ''
+ else:
+ fmt, compr_type = msg.format.split(';')
+ # remove white space
+ fmt = fmt.strip()
+ compr_type = compr_type.strip()
+ return fmt, compr_type
+
+
+def decompresse_imgmsg(msg):
+ if ';' not in msg.format:
+ fmt = ''
+ if msg.format not in ['png', 'jpeg', 'rvl']:
+ raise RuntimeError(
+ 'Unsupported or invalid compresssion format {}'
+ 'Please report this error'
+ 'https://github.com/jsk-ros-pkg/jsk_common/issues/new'
+ .format(msg.format))
+ if msg.format in ['rvl']:
+ compr_type = 'compressedDepth'
+ else:
+ try:
+ return msg_to_bgr(msg, compressed=True)
+ except Exception:
+ pass
+ try:
+ return msg_to_np_depth(msg, compressed=True)
+ except Exception:
+ raise RuntimeError(
+ 'Unsupported or invalid compresssion format {}'
+ 'Please report this error'
+ 'https://github.com/jsk-ros-pkg/jsk_common/issues/new'
+ .format(msg.format))
+ else:
+ fmt, compr_type = compressed_format(msg)
+ if compr_type == 'compressedDepth':
+ return msg_to_np_depth(msg, compressed=True)
+ else:
+ return msg_to_bgr(msg, compressed=True)
+
+
+def msg_to_bgr(msg, compressed=False):
+ if compressed:
+ np_arr = np.fromstring(
+ msg.data, np.uint8)
+ img = cv2.imdecode(
+ np_arr, cv2.IMREAD_COLOR)
+ if msg.format == 'rgb8' or msg.format == 'rgba8':
+ bgr_img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+ else:
+ bgr_img = img
+ else:
+ img = _bridge.imgmsg_to_cv2(
+ msg,
+ desired_encoding=msg.encoding)
+ if msg.encoding == 'rgb8' or msg.encoding == 'rgba8':
+ bgr_img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR)
+ else:
+ bgr_img = img
+ return bgr_img
+
+
+def msg_to_mono(msg, compressed=False):
+ if compressed:
+ np_arr = np.fromstring(msg.data, np.uint8)
+ img = cv2.imdecode(
+ np_arr, cv2.IMREAD_GRAYSCALE)
+ else:
+ img = _bridge.imgmsg_to_cv2(
+ msg,
+ desired_encoding=msg.encoding)
+ return img
+
+
+def msg_to_np_depth(msg, compressed=False, rescale=True):
+ if compressed:
+ # 'msg' as type CompressedImage
+ depth_fmt, compr_type = msg.format.split(';')
+ # remove white space
+ depth_fmt = depth_fmt.strip()
+ compr_type = compr_type.strip()
+ if compr_type != "compressedDepth":
+ raise Exception("Compression type is not 'compressedDepth'."
+ "You probably subscribed to the wrong topic. "
+ "get compresstion_type: {}".format(compr_type))
+
+ # remove header from raw data
+ depth_header_size = 12
+ raw_data = msg.data[depth_header_size:]
+
+ depth_img_raw = cv2.imdecode(np.fromstring(raw_data, np.uint8),
+ cv2.IMREAD_UNCHANGED)
+ if depth_img_raw is None:
+ # probably wrong header size
+ raise Exception("Could not decode compressed depth image."
+ "You may need to change 'depth_header_size'!")
+
+ if depth_fmt == "16UC1":
+ if rescale:
+ depth_img = depth_img_raw / 1000.0
+ else:
+ depth_img = depth_img_raw
+ elif depth_fmt == "32FC1":
+ raw_header = msg.data[:depth_header_size]
+ # header: int, float, float
+ [compfmt, depthQuantA, depthQuantB] = struct.unpack('iff',
+ raw_header)
+ if rescale:
+ depth_img_scaled = depthQuantA / (
+ depth_img_raw.astype(np.float32) - depthQuantB)
+ else:
+ depth_img_scaled = depth_img_raw
+ # filter max values
+ depth_img_scaled[depth_img_raw == 0] = 0
+
+ # depth_img_scaled provides distance in meters as f32
+ # for storing it as png, we need to convert
+ # it to 16UC1 again (depth in mm)
+ depth_img = depth_img_scaled
+ else:
+ raise Exception(
+ "Decoding of '" + depth_fmt + "' is not implemented!")
+ else:
+ depth_img = _bridge.imgmsg_to_cv2(
+ msg,
+ desired_encoding='passthrough')
+ if msg.encoding == '16UC1':
+ if rescale:
+ depth_img = np.asarray(depth_img, dtype=np.float32)
+ depth_img /= 1000.0 # convert metric: mm -> m
+ elif msg.encoding == '32FC1':
+ pass
+ else:
+ print('Unsupported depth encoding: %s'
+ % msg.encoding)
+ return depth_img
+
+
+def compress_img_msg(msg):
+ compressed_msg = sensor_msgs.msg.CompressedImage(
+ header=msg.header)
+ compressed_msg.format = msg.encoding + '; jpeg compressed bgr8'
+
+ img = msg_to_bgr(msg, compressed=False)
+ compressed_msg.data = np.array(cv2.imencode(
+ '.jpg', img)[1]).tostring()
+ return compressed_msg
+
+
+def compress_depth_msg(msg, depth_quantization=100, depth_max=None):
+ compressed_msg = sensor_msgs.msg.CompressedImage(
+ header=msg.header)
+ compressed_msg.format = '{}; compressedDepth'.format(
+ msg.encoding)
+ if msg.encoding == '32FC1':
+ depth = msg_to_np_depth(
+ msg, compressed=False, rescale=True)
+ if depth_max is None:
+ depth_max = depth.max() + 1.0
+ depth_quant_a = depth_quantization * (depth_quantization + 1.0)
+ depth_quant_b = 1.0 - depth_quant_a / depth_max
+ inv_depth_img = np.zeros_like(depth, dtype=np.uint16)
+ target_pixel = np.logical_and(depth_max > depth, depth > 0)
+ inv_depth_img[target_pixel] = depth_quant_a / \
+ depth[target_pixel] + depth_quant_b
+
+ compressed_msg.data = struct.pack(
+ 'iff', 0, depth_quant_a, depth_quant_b)
+ compressed_msg.data += np.array(
+ cv2.imencode('.png', inv_depth_img)[1]).tostring()
+ elif msg.encoding == '16UC1':
+ depth = msg_to_np_depth(
+ msg, compressed=False, rescale=False).copy()
+ if depth_max is None:
+ depth_max = depth.max()
+ depth_max_ushort = depth_max * 1000.0
+ depth[depth > depth_max_ushort] = 0
+
+ compressed_msg.data = " " * 12
+ compressed_msg.data += np.array(
+ cv2.imencode('.png', depth)[1]).tostring()
+ else:
+ raise NotImplementedError(
+ 'Unsupported compressed depth image format {}'
+ .format(msg.encoding))
+
+ return compressed_msg
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/extract.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/extract.py
new file mode 100644
index 000000000..087a9541f
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/extract.py
@@ -0,0 +1,93 @@
+import numpy as np
+import rosbag
+
+from jsk_rosbag_tools.cv import compressed_format
+from jsk_rosbag_tools.cv import decompresse_imgmsg
+from jsk_rosbag_tools.cv import msg_to_img
+from jsk_rosbag_tools.info import get_topic_dict
+
+
+def get_image_topic_names(bag_filepath,
+ rgb_only=False):
+ topic_dict = get_topic_dict(bag_filepath)
+ topic_names = []
+ for topic_name, info in topic_dict.items():
+ if info['type'] == 'sensor_msgs/Image' or \
+ info['type'] == 'sensor_msgs/CompressedImage':
+ if rgb_only:
+ msg = extract_oneshot_topic(bag_filepath, topic_name)
+ if topic_dict[topic_name]['type'] == 'sensor_msgs/Image':
+ encoding = msg.encoding
+ else:
+ encoding, _ = compressed_format(msg)
+ if encoding in ['bgra8', 'bgr8',
+ 'rgba8', 'rgb8']:
+ topic_names.append(topic_name)
+ else:
+ topic_names.append(topic_name)
+ return topic_names
+
+
+def extract_oneshot_topic(bag_filepath, topic_name):
+ topic_dict = get_topic_dict(bag_filepath)
+
+ if topic_name not in topic_dict:
+ raise ValueError('Topic not exists {}'.
+ format(topic_name))
+
+ msg = None
+ with rosbag.Bag(bag_filepath, 'r') as input_rosbag:
+ for topic, msg, stamp in input_rosbag.read_messages(
+ topics=[topic_name, ]):
+ break
+ return msg
+
+
+def extract_image_topic(bag_filepath, topic_name):
+ topic_dict = get_topic_dict(bag_filepath)
+ if topic_name not in topic_dict:
+ raise ValueError("topic ({}) is not included in bagfile ({})."
+ .format(topic_name, bag_filepath))
+
+ with rosbag.Bag(bag_filepath, 'r') as input_rosbag:
+ for topic, msg, _ in input_rosbag.read_messages(
+ topics=[topic_name]):
+ topic_type = topic_dict[topic]['type']
+ if topic_type == 'sensor_msgs/Image':
+ bgr_img = msg_to_img(msg)
+ encoding = msg.encoding
+ elif topic_type == 'sensor_msgs/CompressedImage':
+ bgr_img = decompresse_imgmsg(msg)
+ encoding, _ = compressed_format(msg)
+ else:
+ raise RuntimeError('Unsupported Image topic {}'.format(
+ topic_type))
+
+ # padding image
+ if bgr_img.shape[0] % 2 != 0:
+ if bgr_img.ndim == 2:
+ pad_img = np.zeros(
+ (1, bgr_img.shape[1]), dtype=bgr_img.dtype)
+ elif bgr_img.ndim == 3:
+ pad_img = np.zeros(
+ (1, bgr_img.shape[1], bgr_img.shape[2]),
+ dtype=bgr_img.dtype)
+ else:
+ raise ValueError('Invalid image shape {}'
+ .format(bgr_img.shape))
+ bgr_img = np.concatenate([bgr_img, pad_img], axis=0)
+
+ if bgr_img.shape[1] % 2 != 0:
+ if bgr_img.ndim == 2:
+ pad_img = np.zeros(
+ (bgr_img.shape[0], 1), dtype=bgr_img.dtype)
+ elif bgr_img.ndim == 3:
+ pad_img = np.zeros(
+ (bgr_img.shape[0], 1, bgr_img.shape[2]),
+ dtype=bgr_img.dtype)
+ else:
+ raise ValueError('Invalid image shape {}'
+ .format(bgr_img.shape))
+ bgr_img = np.concatenate([bgr_img, pad_img], axis=1)
+
+ yield msg.header.stamp.to_sec(), topic, bgr_img, encoding
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/image_utils.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/image_utils.py
new file mode 100644
index 000000000..1a8e98f9d
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/image_utils.py
@@ -0,0 +1,72 @@
+import cv2
+import numpy as np
+import PIL
+
+
+def pil_to_cv2_interpolation(interpolation):
+ if isinstance(interpolation, str):
+ interpolation = interpolation.lower()
+ if interpolation == 'nearest':
+ cv_interpolation = cv2.INTER_NEAREST
+ elif interpolation == 'bilinear':
+ cv_interpolation = cv2.INTER_LINEAR
+ elif interpolation == 'bicubic':
+ cv_interpolation = cv2.INTER_CUBIC
+ elif interpolation == 'lanczos':
+ cv_interpolation = cv2.INTER_LANCZOS4
+ else:
+ raise ValueError(
+ 'Not valid Interpolation. '
+ 'Valid interpolation methods are '
+ 'nearest, bilinear, bicubic and lanczos.')
+ else:
+ if interpolation == PIL.Image.NEAREST:
+ cv_interpolation = cv2.INTER_NEAREST
+ elif interpolation == PIL.Image.BILINEAR:
+ cv_interpolation = cv2.INTER_LINEAR
+ elif interpolation == PIL.Image.BICUBIC:
+ cv_interpolation = cv2.INTER_CUBIC
+ elif interpolation == PIL.Image.LANCZOS:
+ cv_interpolation = cv2.INTER_LANCZOS4
+ else:
+ raise ValueError(
+ 'Not valid Interpolation. '
+ 'Valid interpolation methods are '
+ 'PIL.Image.NEAREST, PIL.Image.BILINEAR, '
+ 'PIL.Image.BICUBIC and PIL.Image.LANCZOS.')
+ return cv_interpolation
+
+
+def resize_keeping_aspect_ratio(img, width=None, height=None,
+ interpolation='bilinear'):
+ if (width and height) or (width is None and height is None):
+ raise ValueError('Only width or height should be specified.')
+ if width == img.shape[1] and height == img.shape[0]:
+ return img
+ if width:
+ height = 1.0 * width * img.shape[0] / img.shape[1]
+ else:
+ width = 1.0 * height * img.shape[1] / img.shape[0]
+ height = int(height)
+ width = int(width)
+ cv_interpolation = pil_to_cv2_interpolation(interpolation)
+ return cv2.resize(img, (width, height),
+ interpolation=cv_interpolation)
+
+
+def resize_keeping_aspect_ratio_wrt_target_size(
+ img, width, height, interpolation='bilinear',
+ background_color=(0, 0, 0)):
+ if width == img.shape[1] and height == img.shape[0]:
+ return img
+ H, W, _ = img.shape
+ ratio = min(float(height) / H, float(width) / W)
+ M = np.array([[ratio, 0, 0],
+ [0, ratio, 0]], dtype=np.float32)
+ dst = np.zeros((int(height), int(width), 3), dtype=img.dtype)
+ return cv2.warpAffine(
+ img, M,
+ (int(width), int(height)),
+ dst,
+ cv2.INTER_CUBIC, cv2.BORDER_CONSTANT,
+ background_color)
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/info.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/info.py
new file mode 100644
index 000000000..015489f5c
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/info.py
@@ -0,0 +1,22 @@
+import os
+
+import rosbag
+import yaml
+
+
+def get_info(bag_filepath):
+ if not os.path.exists(bag_filepath):
+ raise OSError('bag file {} not exists'.format(bag_filepath))
+ info_dict = yaml.load(
+ rosbag.Bag(bag_filepath)._get_yaml_info(),
+ Loader=yaml.SafeLoader)
+ return info_dict
+
+
+def get_topic_dict(bag_filepath):
+ info_dict = get_info(bag_filepath)
+ topics = info_dict['topics']
+ topic_dict = {}
+ for topic in topics:
+ topic_dict[topic['topic']] = topic
+ return topic_dict
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/makedirs.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/makedirs.py
new file mode 100644
index 000000000..cc3795eae
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/makedirs.py
@@ -0,0 +1,32 @@
+import os
+import sys
+
+
+PY3 = (sys.version_info[0] == 3)
+PY2 = not PY3
+
+
+def makedirs(name, mode=0o777, exist_ok=True):
+ """An wrapper of os.makedirs that accepts exist_ok.
+
+ Parameters
+ ----------
+ name : str
+ path of directory
+ exist_ok : bool
+ if True, accepts the existence of the directory.
+
+ Examples
+ --------
+ >>> from eos import makedirs
+ >>> makedirs('/tmp/result_directory')
+ """
+ name = str(name)
+ if PY2:
+ try:
+ os.makedirs(name, mode)
+ except OSError:
+ if not (exist_ok and os.path.isdir(name)):
+ raise OSError('Directory {} already exists'.format(name))
+ else:
+ os.makedirs(name, mode, exist_ok=exist_ok)
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/merge.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/merge.py
new file mode 100644
index 000000000..40dc7e24c
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/merge.py
@@ -0,0 +1,83 @@
+import os
+
+import rosbag
+
+
+def get_next(bag_iter, reindex=False,
+ main_start_time=None, start_time=None,
+ topics=None):
+ try:
+ result = next(bag_iter)
+ if topics is not None:
+ while not result[0] in topics:
+ result = next(bag_iter)
+ if reindex:
+ return (result[0], result[1],
+ result[2] - start_time + main_start_time)
+ return result
+ except StopIteration:
+ return None
+
+
+def merge_bag(main_bagfile, bagfile, outfile=None, topics=None,
+ reindex=True):
+ # get min and max time in bagfile
+ main_limits = get_limits(main_bagfile)
+ limits = get_limits(bagfile)
+ # check output file
+ if outfile is None:
+ pattern = main_bagfile + "_merged_%i.bag"
+ outfile = main_bagfile + "_merged.bag"
+ index = 0
+ while (os.path.exists(outfile)):
+ outfile = pattern % index
+ index += 1
+ # output some information
+ print("merge bag %s in %s" % (bagfile, main_bagfile))
+ print("topics filter: ", topics)
+ print("writing to %s." % outfile)
+ # merge bagfile
+ outbag = rosbag.Bag(outfile, 'w')
+ main_bag = rosbag.Bag(main_bagfile).__iter__()
+ bag = rosbag.Bag(bagfile).__iter__()
+ main_next = get_next(main_bag)
+ next = get_next(bag, reindex, main_limits[0], limits[0], topics)
+ try:
+ while main_next is not None or next is not None:
+ if main_next is None:
+ outbag.write(next[0], next[1], next[2])
+ next = get_next(
+ bag,
+ reindex,
+ main_limits[0],
+ limits[0],
+ topics)
+ elif next is None:
+ outbag.write(main_next[0], main_next[1], main_next[2])
+ main_next = get_next(main_bag)
+ elif next[2] < main_next[2]:
+ outbag.write(next[0], next[1], next[2])
+ next = get_next(
+ bag,
+ reindex,
+ main_limits[0],
+ limits[0],
+ topics)
+ else:
+ outbag.write(main_next[0], main_next[1], main_next[2])
+ main_next = get_next(main_bag)
+ finally:
+ outbag.close()
+
+
+def get_limits(bagfile):
+ print("Determine start and end index of %s..." % bagfile)
+ end_time = None
+ start_time = None
+
+ for topic, msg, t in rosbag.Bag(bagfile).read_messages():
+ if start_time is None or t < start_time:
+ start_time = t
+ if end_time is None or t > end_time:
+ end_time = t
+ return (start_time, end_time)
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/topic_name_utils.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/topic_name_utils.py
new file mode 100644
index 000000000..16d190e3a
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/topic_name_utils.py
@@ -0,0 +1,7 @@
+def topic_name_to_file_name(topic_name):
+ if topic_name[0] == '/':
+ replaced_topic_name = topic_name[1:]
+ else:
+ replaced_topic_name = topic_name
+ replaced_topic_name = replaced_topic_name.replace('/', '--slash--')
+ return replaced_topic_name
diff --git a/jsk_rosbag_tools/python/jsk_rosbag_tools/video.py b/jsk_rosbag_tools/python/jsk_rosbag_tools/video.py
new file mode 100644
index 000000000..0e9dfdfc2
--- /dev/null
+++ b/jsk_rosbag_tools/python/jsk_rosbag_tools/video.py
@@ -0,0 +1,243 @@
+from __future__ import division
+
+import datetime
+import math
+import os.path as osp
+import re
+import shutil
+import subprocess
+import sys
+import tempfile
+import time
+import wave
+
+import audio_common_msgs.msg
+import cv2
+from moviepy.editor import VideoFileClip
+import numpy as np
+import rosbag
+import rospy
+from tqdm import tqdm
+
+from jsk_rosbag_tools.cv import img_to_msg
+from jsk_rosbag_tools.merge import merge_bag
+
+
+def to_seconds(date):
+ return time.mktime(date.timetuple())
+
+
+def mediainfo(filepath):
+ prober = 'ffprobe'
+ command_args = [
+ "-v", "quiet",
+ "-show_format",
+ "-show_streams",
+ filepath
+ ]
+
+ command = [prober, '-of', 'old'] + command_args
+ res = subprocess.Popen(command, stdout=subprocess.PIPE)
+ output = res.communicate()[0].decode("utf-8")
+
+ if res.returncode != 0:
+ command = [prober] + command_args
+ output = subprocess.Popen(
+ command,
+ stdout=subprocess.PIPE).communicate()[0].decode("utf-8")
+ rgx = re.compile(r"(?:(?P.*?):)?(?P.*?)\=(?P.*?)$")
+ info = {}
+ if sys.platform == 'win32':
+ output = output.replace("\r", "")
+ for line in output.split("\n"):
+ # print(line)
+ mobj = rgx.match(line)
+
+ if mobj:
+ # print(mobj.groups())
+ inner_dict, key, value = mobj.groups()
+
+ if inner_dict:
+ try:
+ info[inner_dict]
+ except KeyError:
+ info[inner_dict] = {}
+ info[inner_dict][key] = value
+ else:
+ info[key] = value
+
+ return info
+
+
+def nsplit(xlst, n):
+ total_n = len(xlst)
+ d = int((total_n + n - 1) / n)
+ i = 0
+ ret = []
+ while i < total_n:
+ ret.append(xlst[i:i + d])
+ i += d
+ return ret
+
+
+def get_video_duration(video_path):
+ video_path = str(video_path)
+ if not osp.exists(video_path):
+ raise OSError("{} not exists".format(video_path))
+ cap = cv2.VideoCapture(video_path)
+ fps = cap.get(cv2.CAP_PROP_FPS)
+ frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT))
+ cap.release()
+ duration = frame_count / fps
+ return duration
+
+
+def get_video_n_frame(video_path):
+ video_path = str(video_path)
+ if not osp.exists(video_path):
+ raise OSError("{} not exists".format(video_path))
+ clip = VideoFileClip(video_path)
+ return int(clip.duration * clip.fps)
+
+
+def get_video_fps(video_path):
+ video_path = str(video_path)
+ if not osp.exists(video_path):
+ raise OSError("{} not exists".format(video_path))
+ clip = VideoFileClip(video_path)
+ return clip.fps
+
+
+def load_frame(video_path, start=0.0, duration=-1,
+ target_size=None, sampling_frequency=None):
+ video_path = str(video_path)
+ vid = cv2.VideoCapture(video_path)
+ fps = vid.get(cv2.CAP_PROP_FPS)
+ vid.set(cv2.CAP_PROP_POS_MSEC, start)
+ vid_avail = True
+ if sampling_frequency is not None:
+ frame_interval = int(math.ceil(fps * sampling_frequency))
+ else:
+ frame_interval = 1
+ cur_frame = 0
+ while True:
+ stamp = float(cur_frame) / fps
+ vid_avail, frame = vid.read()
+ if not vid_avail:
+ break
+ if duration != -1 and stamp > start + duration:
+ break
+ if target_size is not None:
+ frame = cv2.resize(frame, target_size)
+ yield frame, stamp
+ cur_frame += frame_interval
+ vid.set(cv2.CAP_PROP_POS_FRAMES, cur_frame)
+ vid.release()
+
+
+def count_frames(video_path, start=0.0, duration=-1,
+ sampling_frequency=None):
+ video_duration = get_video_duration(video_path)
+ video_duration -= start
+ if duration > 0:
+ video_duration = max(video_duration - duration, 0)
+ fps = get_video_fps(video_path)
+ if sampling_frequency is not None:
+ return int(math.ceil(
+ video_duration * fps /
+ int(math.ceil(fps * sampling_frequency))))
+ else:
+ return int(math.ceil(video_duration * fps))
+
+
+def video_to_bag(video_filepath, bag_output_filepath,
+ topic_name, compress=False, audio_topic_name='/audio',
+ no_audio=False,
+ base_unixtime=None,
+ fps=None,
+ show_progress_bar=True):
+ if fps is not None:
+ sampling_frequency = 1.0 / fps
+ else:
+ sampling_frequency = None
+ if base_unixtime is None:
+ base_unixtime = to_seconds(datetime.datetime.now())
+
+ topic_name = topic_name.lstrip('/compressed')
+ if compress is True:
+ topic_name = osp.join(topic_name, 'compressed')
+
+ tmpdirname = tempfile.mkdtemp("", 'tmp', None)
+ video_out = osp.join(tmpdirname, 'video.tmp.bag')
+ n_frame = count_frames(video_filepath,
+ sampling_frequency=sampling_frequency)
+ if show_progress_bar:
+ progress = tqdm(total=n_frame)
+ with rosbag.Bag(video_out, 'w') as outbag:
+ for img, timestamp in load_frame(
+ video_filepath,
+ sampling_frequency=sampling_frequency):
+ if show_progress_bar:
+ progress.update(1)
+ msg = img_to_msg(img, compress=compress)
+ sec = int(base_unixtime + timestamp)
+ nsec = ((base_unixtime + timestamp) * (10 ** 9)) % (10 ** 9)
+ ros_timestamp = rospy.rostime.Time(sec, nsec)
+ msg.header.stamp = ros_timestamp
+ outbag.write(topic_name, msg, ros_timestamp)
+ if show_progress_bar:
+ progress.close()
+
+ extract_audio = True
+ if no_audio is False:
+ wav_filepath = osp.join(tmpdirname, 'tmp.wav')
+ cmd = "ffmpeg -i '{}' '{}'".format(
+ video_filepath, wav_filepath)
+ proc = subprocess.Popen(cmd, shell=True,
+ stdout=subprocess.PIPE,
+ stderr=subprocess.PIPE)
+ proc.wait()
+
+ try:
+ wf = wave.open(wav_filepath, mode='rb')
+ sample_rate = wf.getframerate()
+ wf.rewind()
+ buf = wf.readframes(-1)
+ if wf.getsampwidth() == 2:
+ data = np.frombuffer(buf, dtype='int16')
+ elif wf.getsampwidth() == 4:
+ data = np.frombuffer(buf, dtype='int32')
+ data = data.reshape(-1, wf.getnchannels())
+ media_info = mediainfo(wav_filepath)
+ except RuntimeError:
+ extract_audio = False
+
+ if extract_audio:
+ rate = 100
+ n = int(np.ceil(data.shape[0] / (sample_rate // rate)))
+ channels = data.shape[1]
+
+ audio_out = osp.join(tmpdirname, 'audio.tmp.bag')
+ with rosbag.Bag(audio_out, 'w') as outbag:
+ audio_info = audio_common_msgs.msg.AudioInfo(
+ channels=channels,
+ sample_rate=sample_rate,
+ sample_format=media_info['codec_name'].upper(),
+ bitrate=int(media_info['bit_rate']),
+ coding_format='wave')
+ outbag.write(audio_topic_name + '_info',
+ audio_info, ros_timestamp)
+ for i, audio_data in enumerate(nsplit(data, n)):
+ msg = audio_common_msgs.msg.AudioData()
+ msg.data = audio_data.reshape(-1).tobytes()
+ timestamp = i * 0.01
+ sec = int(base_unixtime + timestamp)
+ nsec = (
+ (base_unixtime + timestamp) * (10 ** 9)) % (10 ** 9)
+ ros_timestamp = rospy.rostime.Time(sec, nsec)
+ outbag.write(audio_topic_name, msg, ros_timestamp)
+ merge_bag(video_out, audio_out, bag_output_filepath)
+ else:
+ shutil.move(video_out, bag_output_filepath)
+ else:
+ shutil.move(video_out, bag_output_filepath)
diff --git a/jsk_rosbag_tools/requirements.in b/jsk_rosbag_tools/requirements.in
new file mode 100644
index 000000000..f7b02cd95
--- /dev/null
+++ b/jsk_rosbag_tools/requirements.in
@@ -0,0 +1 @@
+moviepy==1.0.3
diff --git a/jsk_rosbag_tools/samples/data/.gitignore b/jsk_rosbag_tools/samples/data/.gitignore
new file mode 100644
index 000000000..d6b7ef32c
--- /dev/null
+++ b/jsk_rosbag_tools/samples/data/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/jsk_rosbag_tools/scripts/bag_to_audio.py b/jsk_rosbag_tools/scripts/bag_to_audio.py
new file mode 100644
index 000000000..ac3ad9fd8
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/bag_to_audio.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+
+import argparse
+import os.path as osp
+
+import termcolor
+
+from jsk_rosbag_tools.bag_to_audio import bag_to_audio
+
+
+def main():
+ parser = argparse.ArgumentParser(description='rosbag to audio')
+ parser.add_argument('--out', '-o', default='',
+ help='output filename. '
+ 'If `--audio-topic`_info is exists, '
+ "you don't have to specify samplerate and channels.")
+ parser.add_argument('--samplerate', '-r', type=int, help='sampling rate',
+ default='16000')
+ parser.add_argument('--channels',
+ type=int, default=1, help='number of input channels')
+ parser.add_argument('--audio-topic', type=str, default='/audio')
+ parser.add_argument('input_bagfile')
+ args = parser.parse_args()
+
+ if len(args.out) == 0:
+ args.out = osp.join(
+ osp.dirname(args.input_bagfile),
+ osp.splitext(osp.basename(args.input_bagfile))[0] + '.wav')
+
+ audio_exists = bag_to_audio(
+ args.input_bagfile, args.out,
+ samplerate=args.samplerate,
+ channels=args.channels,
+ topic_name=args.audio_topic)
+ if audio_exists is False:
+ termcolor.cprint(
+ '=> Audio topic is not exists', 'red')
+ else:
+ termcolor.cprint(
+ '=> Audio saved to {}'.format(args.out), 'green')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_rosbag_tools/scripts/bag_to_video.py b/jsk_rosbag_tools/scripts/bag_to_video.py
new file mode 100644
index 000000000..c37730197
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/bag_to_video.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import argparse
+import os.path as osp
+
+from jsk_rosbag_tools.bag_to_video import bag_to_video
+
+
+def main():
+ parser = argparse.ArgumentParser(description='rosbag to video')
+ parser.add_argument('--out', '-o', default='',
+ help='output directory path or filename. '
+ 'If more than one --image-topic are specified, '
+ 'this will be interpreted as a directory name. '
+ 'Otherwise this is the file name.')
+ parser.add_argument('--fps', default=30,
+ type=int)
+ parser.add_argument('--samplerate', '-r', type=int, help='sampling rate',
+ default='16000')
+ parser.add_argument('--channels',
+ type=int, default=1, help='number of input channels')
+ parser.add_argument('--audio-topic', type=str, default='/audio')
+ parser.add_argument('--image-topic', type=str, default=[],
+ nargs='+', help='Topic name to extract.')
+ parser.add_argument('--no-progress-bar', action='store_true',
+ help="Don't show progress bar.")
+ parser.add_argument('input_bagfile')
+ args = parser.parse_args()
+
+ if len(args.out) == 0:
+ args.out = osp.join(
+ osp.dirname(args.input_bagfile),
+ osp.splitext(osp.basename(args.input_bagfile))[0])
+ input_bagfile = args.input_bagfile
+ image_topic = None
+ image_topics = None
+ output_filepath = None
+ output_dirpath = None
+ if len(args.image_topic) == 1:
+ image_topic = args.image_topic[0]
+ output_filepath = args.out
+ elif len(args.image_topic) > 1:
+ image_topics = args.image_topic
+ output_dirpath = args.out
+ else:
+ output_dirpath = args.out
+ bag_to_video(input_bagfile,
+ output_filepath=output_filepath,
+ output_dirpath=output_dirpath,
+ image_topic=image_topic,
+ image_topics=image_topics,
+ fps=args.fps,
+ samplerate=args.samplerate,
+ channels=args.channels,
+ audio_topic=args.audio_topic,
+ show_progress_bar=not args.no_progress_bar)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_rosbag_tools/scripts/compress_imgs.py b/jsk_rosbag_tools/scripts/compress_imgs.py
new file mode 100644
index 000000000..3c05ef89d
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/compress_imgs.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+
+import argparse
+import os
+import shutil
+
+import termcolor
+
+from jsk_rosbag_tools.compress import compress_bag_imgs
+
+
+def main():
+ parser = argparse.ArgumentParser(
+ description='Convert Image messages '
+ 'to CompressedImage or CompressedDepthImage')
+ parser.add_argument('input_bagfile', help='input bagfile path')
+ parser.add_argument('--out', '-o', default='',
+ help='output bagfile path')
+ parser.add_argument('--compressed-topics', nargs='*',
+ default=[],
+ help='this image topics are compressed')
+ parser.add_argument('--replace', action='store_true')
+ parser.add_argument('--no-progress-bar', action='store_true',
+ help="Don't show progress bar.")
+ args = parser.parse_args()
+ compressed_topics = [t.lstrip('/') for t in args.compressed_topics]
+
+ input_bagfile = args.input_bagfile
+ if len(args.out) == 0:
+ basename = os.path.basename(input_bagfile)
+ dirname = os.path.dirname(input_bagfile)
+ filename, ext = os.path.splitext(basename)
+ out = os.path.join(dirname, '{}-image-compressed{}'
+ .format(filename, ext))
+ else:
+ out = args.out
+ compress_bag_imgs(input_bagfile, out,
+ compressed_topics=compressed_topics,
+ show_progress_bar=not args.no_progress_bar)
+ if args.replace:
+ termcolor.cprint('=> Replaced to {}'.format(out), 'green')
+ shutil.move(out, input_bagfile)
+ else:
+ termcolor.cprint('=> Saved to {}'.format(out), 'green')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_rosbag_tools/scripts/merge.py b/jsk_rosbag_tools/scripts/merge.py
new file mode 100644
index 000000000..21e0851de
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/merge.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+
+import argparse
+
+from jsk_rosbag_tools.merge import merge_bag
+
+
+def main():
+ parser = argparse.ArgumentParser(description='Merges two bagfiles.')
+ parser.add_argument('--out', '-o',
+ type=str, help='name of the output file',
+ default=None, metavar="output_file")
+ parser.add_argument(
+ '--topics', '-t',
+ type=str,
+ help='topics which should be merged to the main bag',
+ default=None)
+ parser.add_argument('-i', help='reindex bagfile',
+ default=False, action="store_true")
+ parser.add_argument(
+ 'main_bagfile',
+ type=str,
+ help='path to a bagfile, which will be the main bagfile')
+ parser.add_argument(
+ 'bagfile',
+ type=str,
+ help='path to a bagfile which should be merged to the main bagfile')
+ args = parser.parse_args()
+ if args.topics is not None:
+ args.topics = args.topics.split(',')
+ merge_bag(args.main_bagfile,
+ args.bagfile,
+ outfile=args.out,
+ topics=args.topics,
+ reindex=args.i)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/jsk_rosbag_tools/scripts/tf_static_to_tf.py b/jsk_rosbag_tools/scripts/tf_static_to_tf.py
new file mode 100644
index 000000000..512a4bb77
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/tf_static_to_tf.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+import argparse
+import os
+
+import rosbag
+import rospy
+import termcolor
+from tqdm import tqdm
+
+
+def main():
+ parser = argparse.ArgumentParser(
+ description='Convert tf_static to tf and save it as a rosbag')
+ parser.add_argument('input_bagfile', help='input bagfile path')
+ parser.add_argument('--out', '-o', default='',
+ help='output bagfile path')
+ parser.add_argument('--no-progress-bar', action='store_true',
+ help="Don't show progress bar.")
+ args = parser.parse_args()
+
+ input_bagfile = args.input_bagfile
+ if len(args.out) == 0:
+ basename = os.path.basename(input_bagfile)
+ dirname = os.path.dirname(input_bagfile)
+ filename, ext = os.path.splitext(basename)
+ out = os.path.join(dirname, '{}--tf_static--converted{}'
+ .format(filename, ext))
+ else:
+ out = args.out
+
+ input_bag = rosbag.Bag(input_bagfile)
+ tf_static_messages = list(input_bag.read_messages(('/tf_static')))
+
+ with rosbag.Bag(out, 'w') as outbag:
+ if args.no_progress_bar is False:
+ progress = tqdm(total=input_bag.get_message_count())
+ for topic, msg, t in input_bag:
+ # update the progress with a post fix
+ if args.no_progress_bar is False:
+ progress.update(1)
+ progress.set_postfix(time=t)
+ if topic == '/tf':
+ for tsm in tf_static_messages:
+ for tsmtr in tsm.message.transforms:
+ tsmtr.header.stamp = t
+ outbag.write('/tf', tsm.message,
+ t - rospy.Duration(0.1))
+ outbag.write(topic, msg, t)
+ termcolor.cprint('=> Saved to {}'.format(out), 'green')
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_rosbag_tools/scripts/video_to_bag.py b/jsk_rosbag_tools/scripts/video_to_bag.py
new file mode 100644
index 000000000..8accea3c4
--- /dev/null
+++ b/jsk_rosbag_tools/scripts/video_to_bag.py
@@ -0,0 +1,50 @@
+#!/usr/bin/env python
+
+import argparse
+import os.path as osp
+
+from jsk_rosbag_tools.video import video_to_bag
+
+
+def main():
+ parser = argparse.ArgumentParser(
+ description='Convert video to bag.')
+ parser.add_argument('inputvideo')
+ parser.add_argument('--out', '-o', type=str,
+ help='name of the output bag file',
+ default=None, metavar="output_file")
+ parser.add_argument('--topic-name', type=str,
+ default='/video/rgb/image_raw',
+ help='Converted topic name.')
+ parser.add_argument('--fps', type=int,
+ help='Frame Rate.', default=None)
+ parser.add_argument('--compress', action='store_true',
+ help='Compress Image flag.')
+ parser.add_argument('--no-progress-bar', action='store_true',
+ help="Don't show progress bar.")
+ args = parser.parse_args()
+
+ video_path = args.inputvideo
+ if args.out is None:
+ args.out = osp.join(
+ osp.dirname(video_path),
+ osp.splitext(osp.basename(video_path))[0] + '.bag')
+
+ outfile = args.out
+ pattern = str(osp.join(
+ osp.dirname(video_path),
+ osp.splitext(osp.basename(video_path))[0] + "_%i.bag"))
+ index = 0
+ while osp.exists(outfile):
+ outfile = pattern % index
+ index += 1
+ video_to_bag(
+ video_path, outfile,
+ args.topic_name,
+ compress=args.compress,
+ fps=args.fps,
+ show_progress_bar=not args.no_progress_bar)
+
+
+if __name__ == '__main__':
+ main()
diff --git a/jsk_rosbag_tools/setup.py b/jsk_rosbag_tools/setup.py
new file mode 100644
index 000000000..939174bc8
--- /dev/null
+++ b/jsk_rosbag_tools/setup.py
@@ -0,0 +1,12 @@
+from distutils.core import setup
+
+from catkin_pkg.python_setup import generate_distutils_setup
+from setuptools import find_packages
+
+
+d = generate_distutils_setup(
+ packages=find_packages('python'),
+ package_dir={'': 'python'},
+)
+
+setup(**d)
diff --git a/jsk_rosbag_tools/tests/output/.gitignore b/jsk_rosbag_tools/tests/output/.gitignore
new file mode 100644
index 000000000..d6b7ef32c
--- /dev/null
+++ b/jsk_rosbag_tools/tests/output/.gitignore
@@ -0,0 +1,2 @@
+*
+!.gitignore
diff --git a/jsk_rosbag_tools/tests/test_bag_to_video.py b/jsk_rosbag_tools/tests/test_bag_to_video.py
new file mode 100644
index 000000000..a50a7cc13
--- /dev/null
+++ b/jsk_rosbag_tools/tests/test_bag_to_video.py
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+
+import os.path as osp
+import subprocess
+import unittest
+
+import rospkg
+import rospy
+
+
+PKG = 'jsk_rosbag_tools'
+NAME = 'test_bag_to_video'
+
+
+class TestBagToVideo(unittest.TestCase):
+
+ def _check_command(self, cmd):
+ proc = subprocess.Popen(cmd, shell=True,
+ stdout=subprocess.PIPE)
+
+ lines = ''
+ with proc.stdout:
+ for line in iter(proc.stdout.readline, b''):
+ lines += line.decode('utf-8').strip()
+ returncode = proc.wait()
+
+ if returncode != 0:
+ rospy.logerr('{}'.format(lines))
+ rospy.logerr('command {} failed.'.format(cmd))
+ raise RuntimeError('command {} failed.'.format(cmd))
+
+ def test_bag_to_video_and_video_to_bag_and_compress(self):
+ rospack = rospkg.RosPack()
+ path = rospack.get_path('jsk_rosbag_tools')
+
+ output_dir = osp.join(path, 'tests', 'output', 'audio')
+ audio_bag_path = osp.join(path, 'samples', 'data',
+ '2022-05-07-hello-test.bag')
+
+ cmd = 'rosrun jsk_rosbag_tools bag_to_video.py {} -o {}'.format(
+ audio_bag_path, output_dir)
+ self._check_command(cmd)
+
+ output_dir = osp.join(path, 'tests', 'output', 'video')
+ video_bag_path = osp.join(path, 'samples', 'data',
+ '20220530173950_go_to_kitchen_rosbag.bag')
+ cmd = 'rosrun jsk_rosbag_tools bag_to_video.py {} -o {}'.format(
+ video_bag_path, output_dir)
+ self._check_command(cmd)
+
+ # video_to_bag.py test
+ video_path = osp.join(
+ output_dir,
+ 'head_camera--slash--rgb--slash--throttled'
+ '--slash--image_rect_color--slash--compressed.mp4')
+ output_filename = osp.join(path, 'tests', 'output', 'video_to_bag.bag')
+ cmd = 'rosrun jsk_rosbag_tools video_to_bag.py {} ' \
+ '--out {} --no-progress-bar'.format(
+ video_path, output_filename)
+ self._check_command(cmd)
+
+ # compress_imgs.py test
+ cmd = 'rosrun jsk_rosbag_tools compress_imgs.py {} ' \
+ '--no-progress-bar'.format(output_filename)
+ self._check_command(cmd)
+
+
+if __name__ == '__main__':
+ import rostest
+ rostest.rosrun(PKG, NAME, TestBagToVideo)
diff --git a/jsk_rosbag_tools/tests/test_bag_to_video.test b/jsk_rosbag_tools/tests/test_bag_to_video.test
new file mode 100644
index 000000000..df0b7d81e
--- /dev/null
+++ b/jsk_rosbag_tools/tests/test_bag_to_video.test
@@ -0,0 +1,7 @@
+
+
+
+
+
+
diff --git a/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.py b/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.py
new file mode 100644
index 000000000..f992baf31
--- /dev/null
+++ b/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.py
@@ -0,0 +1,60 @@
+#!/usr/bin/env python
+
+import os.path as osp
+import subprocess
+import unittest
+
+import rospkg
+import rospy
+
+
+PKG = 'jsk_rosbag_tools'
+NAME = 'test_jsk_rosbag_tools'
+
+
+class TestJSKRosBagTools(unittest.TestCase):
+
+ def _check_command(self, cmd):
+ proc = subprocess.Popen(cmd, shell=True,
+ stdout=subprocess.PIPE)
+ with proc.stdout:
+ for line in iter(proc.stdout.readline, b''):
+ rospy.loginfo('{}'.format(line.decode('utf-8').strip()))
+ returncode = proc.wait()
+
+ if returncode != 0:
+ raise RuntimeError('command {} failed.'.format(cmd))
+
+ def test_bag_to_audio(self):
+ rospack = rospkg.RosPack()
+ path = rospack.get_path('jsk_rosbag_tools')
+ video_bag_path = osp.join(path, 'samples', 'data',
+ '20220530173950_go_to_kitchen_rosbag.bag')
+ cmd = 'rosrun jsk_rosbag_tools bag_to_audio.py {}'.format(
+ video_bag_path)
+ self._check_command(cmd)
+
+ def test_tf_static_to_tf(self):
+ rospack = rospkg.RosPack()
+ path = rospack.get_path('jsk_rosbag_tools')
+ video_bag_path = osp.join(path, 'samples', 'data',
+ '20220530173950_go_to_kitchen_rosbag.bag')
+ cmd = 'rosrun jsk_rosbag_tools tf_static_to_tf.py {} ' \
+ '--no-progress-bar'.format(video_bag_path)
+ self._check_command(cmd)
+
+ def test_merge(self):
+ rospack = rospkg.RosPack()
+ path = rospack.get_path('jsk_rosbag_tools')
+
+ output_path = osp.join(path, 'tests', 'output', 'merged.bag')
+ audio_bag_path = osp.join(path, 'samples', 'data',
+ '2022-05-07-hello-test.bag')
+ cmd = 'rosrun jsk_rosbag_tools merge.py {} {} -o {}'\
+ .format(audio_bag_path, audio_bag_path, output_path)
+ self._check_command(cmd)
+
+
+if __name__ == '__main__':
+ import rostest
+ rostest.rosrun(PKG, NAME, TestJSKRosBagTools)
diff --git a/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.test b/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.test
new file mode 100644
index 000000000..9223fa61f
--- /dev/null
+++ b/jsk_rosbag_tools/tests/test_jsk_rosbag_tools.test
@@ -0,0 +1,7 @@
+
+
+
+
+
+