From b9bc0c60c6257f69f43e419dcda22d56217b5bea Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 9 Aug 2022 12:22:40 +0900 Subject: [PATCH 01/33] rostwitter : install resource directory --- rostwitter/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rostwitter/CMakeLists.txt b/rostwitter/CMakeLists.txt index 7f323bafc..39258afbd 100644 --- a/rostwitter/CMakeLists.txt +++ b/rostwitter/CMakeLists.txt @@ -40,7 +40,7 @@ else() ) endif() -install(DIRECTORY test +install(DIRECTORY test resource DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS ) From 11cc4169159137fb7fdc35a1e340cf683fc58d0b Mon Sep 17 00:00:00 2001 From: Aoi Nakane Date: Sat, 1 Oct 2022 09:28:19 +0900 Subject: [PATCH 02/33] [respeaker_ros] Stop timer on shutdown --- respeaker_ros/scripts/respeaker_node.py | 1 + 1 file changed, 1 insertion(+) diff --git a/respeaker_ros/scripts/respeaker_node.py b/respeaker_ros/scripts/respeaker_node.py index b773b3cb7..bf14ad478 100755 --- a/respeaker_ros/scripts/respeaker_node.py +++ b/respeaker_ros/scripts/respeaker_node.py @@ -349,6 +349,7 @@ def __init__(self): self.sub_led = rospy.Subscriber("status_led", ColorRGBA, self.on_status_led) def on_shutdown(self): + self.info_timer.shutdown() try: self.respeaker.close() except: From 0d1350ef1ac04b2284f08e1eae3dd9c8a014ae5a Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 29 Nov 2022 16:33:50 +0900 Subject: [PATCH 03/33] save google chat image in /chat_notification with timestamp --- google_chat_ros/scripts/google_chat_ros_node.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/google_chat_ros/scripts/google_chat_ros_node.py b/google_chat_ros/scripts/google_chat_ros_node.py index 30a83171c..b47079171 100644 --- a/google_chat_ros/scripts/google_chat_ros_node.py +++ b/google_chat_ros/scripts/google_chat_ros_node.py @@ -31,6 +31,8 @@ def __init__(self): self.download_data = rospy.get_param('~download_data', True) self.download_directory = rospy.get_param('~download_directory', "/tmp") self.download_avatar = rospy.get_param('~download_avatar', False) + self.upload_parents_path = rospy.get_param( + '~upload_parents_path', 'chat_notification') # For sending message self._client = GoogleChatRESTClient(google_credentials) @@ -411,7 +413,11 @@ def _upload_file(self, filepath, return_id=False): return # upload try: - res = gdrive_upload(file_path=filepath) + res = gdrive_upload( + file_path=filepath, + parents_path=self.upload_parents_path, + use_timestamp_folder=True, + ) except rospy.ServiceException as e: rospy.logerr("Failed to call Google Drive upload service, status:{}".format(str(e))) else: From 23649c4f029b9ce72b5009a3d4543cbc8d8587a0 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 29 Nov 2022 18:11:30 +0900 Subject: [PATCH 04/33] use same timestamp for one goal --- .../scripts/google_chat_ros_node.py | 44 +++++++++++++------ 1 file changed, 30 insertions(+), 14 deletions(-) diff --git a/google_chat_ros/scripts/google_chat_ros_node.py b/google_chat_ros/scripts/google_chat_ros_node.py index b47079171..64e6d011f 100644 --- a/google_chat_ros/scripts/google_chat_ros_node.py +++ b/google_chat_ros/scripts/google_chat_ros_node.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import datetime import gdown import json import os @@ -109,6 +110,7 @@ def rest_cb(self, goal): # Card json_body['cards'] = [] + timestamp = '{0:%Y%m%d%H%M%S}'.format(datetime.datetime.now()) if goal.cards: if goal.update_message: json_body['actionResponse'] = {"type": "UPDATE_MESSAGE"} @@ -124,10 +126,12 @@ def rest_cb(self, goal): if card.header.image_url: header['imageUrl'] = card.header.image_url elif card.header.image_filepath: - header['imageUrl'] = self._upload_file(card.header.image_filepath) + header['imageUrl'] = self._upload_file( + card.header.image_filepath, timestamp=timestamp) # card/sections sections = [] - sections = self._make_sections_json(card.sections) + sections = self._make_sections_json( + card.sections, timestamp=timestamp) # card/actions card_actions = [] for card_action_msg in card.card_actions: @@ -269,7 +273,7 @@ def _make_message_msg(self, event): return message - def _make_sections_json(self, sections_msg): + def _make_sections_json(self, sections_msg, timestamp=None): """ :type msg: list of google_chat_ros.msgs/Section :rtype json_body: list of json @@ -279,11 +283,12 @@ def _make_sections_json(self, sections_msg): section = {} if msg.header: section['header'] = msg.header - section['widgets'] = self._make_widget_markups_json(msg.widgets) + section['widgets'] = self._make_widget_markups_json( + msg.widgets, timestamp=timestamp) json_body.append(section) return json_body - def _make_widget_markups_json(self, widgets_msg): + def _make_widget_markups_json(self, widgets_msg, timestamp=None): """Make widget markup json lists. See https://developers.google.com/chat/api/reference/rest/v1/cards#widgetmarkup for details. :rtype widgets_msg: list of google_chat_ros.msgs/WidgetMarkup @@ -298,7 +303,8 @@ def _make_widget_markups_json(self, widgets_msg): buttons = [] buttons_msg = msg.buttons for button_msg in buttons_msg: - buttons.append(self._make_button_json(button_msg)) + buttons.append( + self._make_button_json(button_msg, timestamp=timestamp)) if buttons: json_body.append({'buttons':buttons}) @@ -311,7 +317,8 @@ def _make_widget_markups_json(self, widgets_msg): if msg.image.image_url: image_json['imageUrl'] = msg.image.image_url elif msg.image.localpath: - image_json['imageUrl'] = self._upload_file(msg.image.localpath) + image_json['imageUrl'] = self._upload_file( + msg.image.localpath, timestamp=timestamp) if msg.image.on_click.action.action_method_name or msg.image.on_click.open_link_url: image_json['onClick'] = self._make_on_click_json(msg.image.on_click) if msg.image.aspect_ratio: @@ -330,9 +337,12 @@ def _make_widget_markups_json(self, widgets_msg): elif msg.key_value.original_icon_url: keyval_json['iconUrl'] = msg.key_value.original_icon_url elif msg.key_value.original_icon_localpath: - keyval_json['iconUrl'] = self._upload_file(msg.key_value.original_icon_localpath) + keyval_json['iconUrl'] = self._upload_file( + msg.key_value.original_icon_localpath, + timestamp=timestamp) if msg.key_value.button.text_button_name or msg.key_value.button.image_button_name: - keyval_json['button'] = self._make_button_json(msg.key_value.button) + keyval_json['button'] = self._make_button_json( + msg.key_value.button, timestamp=timestamp) json_body.append({'keyValue':keyval_json}) return json_body @@ -357,7 +367,7 @@ def _make_on_click_json(self, on_click_msg): json_body['openLink'] = {'url': on_click_msg.open_link_url} return json_body - def _make_button_json(self, button_msg): + def _make_button_json(self, button_msg, timestamp=None): """Make button json. See https://developers.google.com/chat/api/reference/rest/v1/cards#button for details. :rtype button_msg: google_chat_ros.msg/Button.msg @@ -381,7 +391,8 @@ def _make_button_json(self, button_msg): elif button_msg.original_icon_url: image_icon['iconUrl'] = button_msg.original_icon_url elif button_msg.original_icon_filepath: - image_icon['iconUrl'] = self._upload_file(button_msg.original_icon_filepath) + image_icon['iconUrl'] = self._upload_file( + button_msg.original_icon_filepath, timestamp=timestamp) return json_body def _get_user_info(self, item): @@ -396,7 +407,7 @@ def _get_user_info(self, item): user.human = True if item.get('type') == "HUMAN" else False return user - def _upload_file(self, filepath, return_id=False): + def _upload_file(self, filepath, return_id=False, timestamp=None): """Get local filepath and upload to Google Drive :param filepath: local file's path you want to upload :type filepath: string @@ -412,11 +423,16 @@ def _upload_file(self, filepath, return_id=False): rospy.logerr(e) return # upload + if timestamp is None: + parents_path = self.upload_parents_path + else: + parents_path = '{}/{}'.format( + self.upload_parents_path, timestamp) try: res = gdrive_upload( file_path=filepath, - parents_path=self.upload_parents_path, - use_timestamp_folder=True, + parents_path=parents_path, + use_timestamp_folder=False, ) except rospy.ServiceException as e: rospy.logerr("Failed to call Google Drive upload service, status:{}".format(str(e))) From fac0764a473a4c84068e99b71c91135a203ccb4d Mon Sep 17 00:00:00 2001 From: JSK PR2 Users Date: Mon, 5 Dec 2022 14:35:45 +0900 Subject: [PATCH 05/33] support indigo in dialogflow_client --- dialogflow_task_executive/node_scripts/dialogflow_client.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/dialogflow_task_executive/node_scripts/dialogflow_client.py b/dialogflow_task_executive/node_scripts/dialogflow_client.py index 57faecd01..98b99c15e 100644 --- a/dialogflow_task_executive/node_scripts/dialogflow_client.py +++ b/dialogflow_task_executive/node_scripts/dialogflow_client.py @@ -107,7 +107,10 @@ def make_dialog_msg(self, result): rospy.logwarn("Unknown action") msg.action = result.action - if self.language == 'ja-JP' and os.environ["ROS_PYTHON_VERSION"] == "2": + # check if ROS_PYTHON_VERSION exists in indigo + if (self.language == 'ja-JP' + and ("ROS_PYTHON_VERSION" not in os.environ + or os.environ["ROS_PYTHON_VERSION"] == "2")): msg.query = result.query_text.encode("utf-8") msg.response = result.fulfillment_text.encode("utf-8") else: From 39e5e492ce23b788b33b668e9b20c05fe105c213 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 15:15:54 +0900 Subject: [PATCH 06/33] add confidence field in speech_recognition_node.py --- .../scripts/speech_recognition_node.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ros_speech_recognition/scripts/speech_recognition_node.py b/ros_speech_recognition/scripts/speech_recognition_node.py index 8ad9788bf..1a9426580 100644 --- a/ros_speech_recognition/scripts/speech_recognition_node.py +++ b/ros_speech_recognition/scripts/speech_recognition_node.py @@ -260,7 +260,10 @@ def audio_cb(self, _, audio): self.play_sound("recognized", 0.05) rospy.loginfo("Result: %s" % result.encode('utf-8')) self.play_sound("success", 0.1) - msg = SpeechRecognitionCandidates(transcript=[result]) + msg = SpeechRecognitionCandidates( + transcript=[result], + confidence=[1.0], + ) self.pub.publish(msg) return except SR.UnknownValueError as e: @@ -326,7 +329,10 @@ def speech_recognition_srv_cb(self, req): rospy.loginfo("Result: %s" % result.encode('utf-8')) if not req.quiet: self.play_sound("success", 0.1) - res.result = SpeechRecognitionCandidates(transcript=[result]) + res.result = SpeechRecognitionCandidates( + transcript=[result], + confidence=[1.0], + ) return res except SR.UnknownValueError: if self.dynamic_energy_threshold: From 90640d6128b2dadc72fba3a3a87b77aac5647674 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 15:17:00 +0900 Subject: [PATCH 07/33] add confidence in speech_to_text.py --- respeaker_ros/scripts/speech_to_text.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/respeaker_ros/scripts/speech_to_text.py b/respeaker_ros/scripts/speech_to_text.py index 439652ba8..0974b2f65 100755 --- a/respeaker_ros/scripts/speech_to_text.py +++ b/respeaker_ros/scripts/speech_to_text.py @@ -83,7 +83,10 @@ def audio_cb(self, msg): rospy.loginfo("Waiting for result %d" % len(data.get_raw_data())) result = self.recognizer.recognize_google( data, language=self.language) - msg = SpeechRecognitionCandidates(transcript=[result]) + msg = SpeechRecognitionCandidates( + transcript=[result], + confidence=[1.0], + ) self.pub_speech.publish(msg) except SR.UnknownValueError as e: rospy.logerr("Failed to recognize: %s" % str(e)) From d1b0d8f09b7497c699556bb9645a7011f6a33150 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 16:04:20 +0900 Subject: [PATCH 08/33] add self cancellation for speech recogntion --- .../launch/speech_recognition.launch | 7 +++ .../scripts/speech_recognition_node.py | 51 +++++++++++++++++++ 2 files changed, 58 insertions(+) diff --git a/ros_speech_recognition/launch/speech_recognition.launch b/ros_speech_recognition/launch/speech_recognition.launch index 0606a8348..d9a6c2693 100644 --- a/ros_speech_recognition/launch/speech_recognition.launch +++ b/ros_speech_recognition/launch/speech_recognition.launch @@ -12,6 +12,10 @@ + + + + diff --git a/ros_speech_recognition/scripts/speech_recognition_node.py b/ros_speech_recognition/scripts/speech_recognition_node.py index 8ad9788bf..5aa2e0e8b 100644 --- a/ros_speech_recognition/scripts/speech_recognition_node.py +++ b/ros_speech_recognition/scripts/speech_recognition_node.py @@ -14,6 +14,7 @@ from audio_common_msgs.msg import AudioData from sound_play.msg import SoundRequest, SoundRequestAction, SoundRequestGoal +from actionlib_msgs.msg import GoalStatus from speech_recognition_msgs.msg import SpeechRecognitionCandidates from speech_recognition_msgs.srv import SpeechRecognition from speech_recognition_msgs.srv import SpeechRecognitionResponse @@ -143,6 +144,31 @@ def __init__(self): "/usr/share/sounds/ubuntu/stereo/window-slide.ogg"), } + + # ignore voice input while the robot is speaking + self.self_cancellation = rospy.get_param("~self_cancellation", True) + # time to assume as SPEAKING after tts service is finished + self.tts_tolerance = rospy.Duration.from_sec( + rospy.get_param("~tts_tolerance", 1.0)) + tts_action_names = rospy.get_param( + '~tts_action_names', ['sound_play']) + + self.tts_action = None + self.last_tts = None + self.is_canceling = False + self.tts_actions = [] + if self.self_cancellation: + for tts_action_name in tts_action_names: + tts_action = actionlib.SimpleActionClient( + tts_action_name, SoundRequestAction) + if tts_action.wait_for_server(rospy.Duration(5.0)): + self.tts_actions.append(tts_action) + else: + rospy.logerr( + "action '{}' is not initialized." + .format(tts_action_name)) + self.tts_timer = rospy.Timer(rospy.Duration(0.1), self.tts_timer_cb) + self.dyn_srv = Server(Config, self.config_callback) self.stop_fn = None @@ -254,6 +280,9 @@ def recognize(self, audio): def audio_cb(self, _, audio): if not self.enable_audio_cb: return + if self.is_canceling: + rospy.loginfo("Robot is speaking now, so recognition is cancelled") + return try: rospy.logdebug("Waiting for result... (Sent %d bytes)" % len(audio.get_raw_data())) result = self.recognize(audio) @@ -360,6 +389,28 @@ def spin(self): self.start_speech_recognition() rospy.spin() + def tts_timer_cb(self, event): + stamp = event.current_real + active = False + for tts_action in self.tts_actions: + for st in tts_action.action_client.last_status_msg.status_list: + if st.status == GoalStatus.ACTIVE: + active = True + break + if active: + break + if active: + if not self.is_canceling: + rospy.logdebug("START CANCELLATION") + self.is_canceling = True + self.last_tts = None + elif self.is_canceling: + if self.last_tts is None: + self.last_tts = stamp + if stamp - self.last_tts > self.tts_tolerance: + rospy.logdebug("END CANCELLATION") + self.is_canceling = False + if __name__ == '__main__': rospy.init_node("speech_recognition") From fd384c14dbeaa9f9f04fc84914cf2e01f9b421ee Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 16:39:36 +0900 Subject: [PATCH 09/33] add readme for self cancellation --- ros_speech_recognition/README.md | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ros_speech_recognition/README.md b/ros_speech_recognition/README.md index d29c183f4..291c35502 100644 --- a/ros_speech_recognition/README.md +++ b/ros_speech_recognition/README.md @@ -148,6 +148,24 @@ This package uses Python package [SpeechRecognition](https://pypi.python.org/pyp Selecting to use topic or service. By default, service is used. +* `~self_cancellation` (`Bool`, default: `True`) + + Whether the node recognize the sound heard when `~tts_action_names` is running or not. + + This options is for ignoring self voice sounds from recognition. + +* `~tts_action_names` (`List[String]`, default: `['sound_play']`) + + Text-to-speech action name for self cancellation. + + The node ignores the voice heard when these Text-to-speech action is running. + +* `~tts_tolerance` (`Float`, default: `1.0`) + + Tolerance seconds for self cancellation. + + The node ignores the voice with this tolerance seconds after `~tts_action_names` finish running. + * `~google_key` (`String`, default: `None`) Auth Key for Google API. If `None`, use public key. (No guarantee to be blocked.) From 5c2cef83dd064932aba53c6ca43810fe5bab22f5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 30 Nov 2022 15:47:30 +0900 Subject: [PATCH 10/33] fix googleapis-common-protos version for noetic --- dialogflow_task_executive/requirements.in.noetic | 1 + 1 file changed, 1 insertion(+) diff --git a/dialogflow_task_executive/requirements.in.noetic b/dialogflow_task_executive/requirements.in.noetic index daf28ef50..7fe2d496f 100644 --- a/dialogflow_task_executive/requirements.in.noetic +++ b/dialogflow_task_executive/requirements.in.noetic @@ -1,4 +1,5 @@ dialogflow==1.1.1 google-api-core[grpc]==1.33.0 grpcio-status==1.48.1 +googleapis-common-protos[grpc]==1.56.2 protobuf==3.20.1 # fix Could not find a version that matches protobuf<4.0.0dev,<5.0.0dev,>=3.15.0,>=3.20.1,>=4.21.3 (from google-api-core[grpc]==1.33.1->dialogflow==1.1.1->-r requirements.in (line 1)) From e84dcb2bd9edec31572af2291070a4af5dc6c104 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 14:42:03 +0900 Subject: [PATCH 11/33] remove unused run_depend in ros_speech_recognition speech_recognition is installed via virtualenv --- ros_speech_recognition/package.xml | 1 - 1 file changed, 1 deletion(-) diff --git a/ros_speech_recognition/package.xml b/ros_speech_recognition/package.xml index d75db7708..ccd2bed12 100644 --- a/ros_speech_recognition/package.xml +++ b/ros_speech_recognition/package.xml @@ -20,7 +20,6 @@ audio_common_msgs dynamic_reconfigure flac - python-speechrecognition-pip sound_play speech_recognition_msgs From 3ceb99827abf5730604053ced881e29c7cccd3a2 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 14:44:46 +0900 Subject: [PATCH 12/33] use catkin_virtualenv for respeaker_ros --- respeaker_ros/CMakeLists.txt | 23 +++++++++++++++++++++-- respeaker_ros/package.xml | 9 ++++++++- respeaker_ros/requirements.txt | 1 + respeaker_ros/scripts/respeaker_gencfg.py | 0 respeaker_ros/scripts/respeaker_node.py | 0 respeaker_ros/scripts/speech_to_text.py | 0 6 files changed, 30 insertions(+), 3 deletions(-) create mode 100644 respeaker_ros/requirements.txt mode change 100755 => 100644 respeaker_ros/scripts/respeaker_gencfg.py mode change 100755 => 100644 respeaker_ros/scripts/respeaker_node.py mode change 100755 => 100644 respeaker_ros/scripts/speech_to_text.py diff --git a/respeaker_ros/CMakeLists.txt b/respeaker_ros/CMakeLists.txt index 59c6034b0..390a82e0f 100644 --- a/respeaker_ros/CMakeLists.txt +++ b/respeaker_ros/CMakeLists.txt @@ -1,7 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(respeaker_ros) -find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure) +find_package(catkin REQUIRED COMPONENTS + catkin_virtualenv + dynamic_reconfigure +) generate_dynamic_reconfigure_options( cfg/Respeaker.cfg @@ -9,14 +12,30 @@ generate_dynamic_reconfigure_options( catkin_package() +if($ENV{ROS_DISTRO} STRGREATER "melodic") + catkin_generate_virtualenv( + PYTHON_INTERPRETER python3 + ) +else() + catkin_generate_virtualenv( + PYTHON_INTERPRETER python2 + ) +endif() + install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) +install(FILES requirements.txt + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + file(GLOB PYTHON_SCRIPTS scripts/*.py) catkin_install_python(PROGRAMS ${PYTHON_SCRIPTS} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts/) if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) - add_rostest(test/sample_respeaker.test) + add_rostest(test/sample_respeaker.test + DEPENDENCIES ${PROJECT_NAME}_generate_virtualenv + ) endif() diff --git a/respeaker_ros/package.xml b/respeaker_ros/package.xml index a8d193b72..ac83b898a 100644 --- a/respeaker_ros/package.xml +++ b/respeaker_ros/package.xml @@ -8,7 +8,9 @@ Yuki Furuta catkin + catkin_virtualenv dynamic_reconfigure + angles audio_common_msgs dynamic_reconfigure @@ -23,6 +25,11 @@ python-pyaudio python3-pyaudio python-pyusb-pip - python-speechrecognition-pip + jsk_tools + + + requirements.txt + + diff --git a/respeaker_ros/requirements.txt b/respeaker_ros/requirements.txt new file mode 100644 index 000000000..7362afdbf --- /dev/null +++ b/respeaker_ros/requirements.txt @@ -0,0 +1 @@ +SpeechRecognition==3.8.1 diff --git a/respeaker_ros/scripts/respeaker_gencfg.py b/respeaker_ros/scripts/respeaker_gencfg.py old mode 100755 new mode 100644 diff --git a/respeaker_ros/scripts/respeaker_node.py b/respeaker_ros/scripts/respeaker_node.py old mode 100755 new mode 100644 diff --git a/respeaker_ros/scripts/speech_to_text.py b/respeaker_ros/scripts/speech_to_text.py old mode 100755 new mode 100644 From 276c6f1e2786c2f008fd5810ded0c385fb24e427 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 17:53:34 +0900 Subject: [PATCH 13/33] no build respeaker_ros in indigo --- .github/workflows/config.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/config.yml b/.github/workflows/config.yml index 0a0602191..deea9b85f 100644 --- a/.github/workflows/config.yml +++ b/.github/workflows/config.yml @@ -20,7 +20,7 @@ jobs: ROS_PARALLEL_TEST_JOBS: "-j8" CATKIN_PARALLEL_JOBS: "-i" NOT_TEST_INSTALL : true - BEFORE_SCRIPT : "for name in dialogflow_task_executive ros_speech_recognition sesame_ros ffha libsiftfast nlopt julius julius_ros downward assimp_devel google_chat_ros; do echo \\$name; find $GITHUB_WORKSPACE -iname \\$name -exec touch {}/CATKIN_IGNORE \\; ; ls -al \\$(find -iname \\$name)/; done" # Skip large packagses / Skip packages that could not build on indigo dialogflow_task_executive/ros_speech_recognition/sesame_ros + BEFORE_SCRIPT : "for name in dialogflow_task_executive ros_speech_recognition sesame_ros respeaker_ros ffha libsiftfast nlopt julius julius_ros downward assimp_devel google_chat_ros; do echo \\$name; find $GITHUB_WORKSPACE -iname \\$name -exec touch {}/CATKIN_IGNORE \\; ; ls -al \\$(find -iname \\$name)/; done" # Skip large packagses / Skip packages that could not build on indigo dialogflow_task_executive/ros_speech_recognition/sesame_ros/respeaker_ros - ROS_DISTRO: kinetic CONTAINER: ubuntu:16.04 ROS_PARALLEL_TEST_JOBS: "-j8" From 40777068c9ffdcd881e89015409c9dd9eae33263 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 15:29:57 +0900 Subject: [PATCH 14/33] update default arg for speech_recognition.launch with this change, speech_recognition.launch automatically starts and recognized results are published in /speech_to_text topic --- ros_speech_recognition/launch/speech_recognition.launch | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ros_speech_recognition/launch/speech_recognition.launch b/ros_speech_recognition/launch/speech_recognition.launch index d9a6c2693..549ae84b2 100644 --- a/ros_speech_recognition/launch/speech_recognition.launch +++ b/ros_speech_recognition/launch/speech_recognition.launch @@ -3,14 +3,15 @@ - + - + + @@ -50,6 +51,7 @@ engine: $(arg engine) language: $(arg language) continuous: $(arg continuous) + auto_start: $(arg auto_start) enable_sound_effect: $(arg launch_sound_play) self_cancellation: $(arg self_cancellation) tts_tolerance: $(arg tts_tolerance) From 4e88c6efaf8791c3c7ab123efefef7a2c4d7201f Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 16:31:03 +0900 Subject: [PATCH 15/33] change the default value for ~voice_topic --- ros_speech_recognition/scripts/speech_recognition_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_speech_recognition/scripts/speech_recognition_node.py b/ros_speech_recognition/scripts/speech_recognition_node.py index 5aa2e0e8b..b6e9bd62b 100644 --- a/ros_speech_recognition/scripts/speech_recognition_node.py +++ b/ros_speech_recognition/scripts/speech_recognition_node.py @@ -178,7 +178,7 @@ def __init__(self): if self.continuous: rospy.loginfo("Enabled continuous mode") rospy.loginfo("Auto start: {}".format(self.auto_start)) - self.pub = rospy.Publisher(rospy.get_param("~voice_topic", "/Tablet/voice"), + self.pub = rospy.Publisher(rospy.get_param("~voice_topic", "speech_to_text"), SpeechRecognitionCandidates, queue_size=1) self.start_srv = rospy.Service( From 9a966f5a60f96e2f260efdf562009418bac20860 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Tue, 6 Dec 2022 16:31:17 +0900 Subject: [PATCH 16/33] update readme for speech recognition --- ros_speech_recognition/README.md | 56 ++++++++++++++++++++++---------- 1 file changed, 39 insertions(+), 17 deletions(-) diff --git a/ros_speech_recognition/README.md b/ros_speech_recognition/README.md index 291c35502..b470ef0e4 100644 --- a/ros_speech_recognition/README.md +++ b/ros_speech_recognition/README.md @@ -1,5 +1,4 @@ -ros_speech_recognition -====================== +# ros\_speech\_recognition A ROS package for speech-to-text services. This package uses Python package [SpeechRecognition](https://pypi.python.org/pypi/SpeechRecognition) as a backend. @@ -18,40 +17,63 @@ This package uses Python package [SpeechRecognition](https://pypi.python.org/pyp roslaunch ros_speech_recognition speech_recognition.launch ``` -3. Use from Python +3. Echo `/speech_to_text` - ```python - import rospy - from ros_speech_recognition import SpeechRecognitionClient - - rospy.init_node("client") - client = SpeechRecognitionClient() - result = client.recognize() # Please say 'Hello, world!' towards microphone - print result # => 'Hello, world!' + ```bash + rostopic echo /speech_to_text + # you can get the recognition result ``` -## Interface +## `speech_recognition_node.py` Interface ### Publishing Topics +* `~voice_topic` (`sound_recognition_msgs/SpeechReconitionCandidates`) + + Speech recognition candidates topic name. + + Topic name is set by parameter `~voice_topic`, and default value is `speech_to_text`. + * `sound_play` (`sound_play/SoundRequestAction`) Action client to play sound on events. If the action server is not available or `~enable_sound_effect` is `False`, no sound is played. ### Subscribing Topics -* `audio` (`audio_common_msgs/AudioData`) +* `~audio_topic` (`audio_common_msgs/AudioData`) Audio stream data to be recognized. + Topis name is set by parameter `~audio_topic` and default value is `audio`. + ### Advertising Services * `speech_recognition` (`speech_recognition_msgs/SpeechRecognition`) Service for speech recognition +* `speech_recognition/start` (`std_srvs/Empty`) + + Start service for speech recognition + + This service is available when parameter `~contiunous` is `True`. + +* `speech_recognition/start` (`std_srvs/Empty`) + + Stop service for speech recognition + + This service is available when parameter `~contiunous` is `True`. + ## Parameters +* `~voice_topic` (`String`, default: `speech_to_text`) + + Publishing voice topic name + +* `~audio_topic` (`String`, default: `audio`) + + Subscribing audio topic name + * `~enable_sound_effect` (`Bool`, default: `True`) Flag to enable or disable sound to play sound on recognition. @@ -108,10 +130,6 @@ This package uses Python package [SpeechRecognition](https://pypi.python.org/pyp Seconds of waiting for speech -* `~audio_topic` (`String`, default: `audio`) - - Topic name of input audio data - * `~depth` (`Int`, default: `16`) Depth of audio signal @@ -148,6 +166,10 @@ This package uses Python package [SpeechRecognition](https://pypi.python.org/pyp Selecting to use topic or service. By default, service is used. +* `~auto_start` (`Bool`, default: True) + + Starting the speech recognition when launching. + * `~self_cancellation` (`Bool`, default: `True`) Whether the node recognize the sound heard when `~tts_action_names` is running or not. From 93a425d184dacbfe9ba5909e60a2e51240e06e16 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Thu, 8 Dec 2022 14:28:21 +0900 Subject: [PATCH 17/33] set voice_topic to /Tablet/voice --- ros_speech_recognition/test/sample_ros_speech_recognition.test | 1 + 1 file changed, 1 insertion(+) diff --git a/ros_speech_recognition/test/sample_ros_speech_recognition.test b/ros_speech_recognition/test/sample_ros_speech_recognition.test index 0a11e404d..57bf3a7e0 100644 --- a/ros_speech_recognition/test/sample_ros_speech_recognition.test +++ b/ros_speech_recognition/test/sample_ros_speech_recognition.test @@ -13,6 +13,7 @@ + From c874a8b2ce2f4faccc10a4290ef5d79383cadb0f Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Wed, 25 Jan 2023 12:08:12 +0900 Subject: [PATCH 18/33] fix google_chat_ros noetic build errors --- google_chat_ros/requirements.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/google_chat_ros/requirements.in b/google_chat_ros/requirements.in index 439fb3fc8..a19cdef7b 100644 --- a/google_chat_ros/requirements.in +++ b/google_chat_ros/requirements.in @@ -7,7 +7,7 @@ google-auth-oauthlib==0.5.2 google-auth==2.8.0 google-cloud-pubsub==2.12.1 googleapis-common-protos[grpc]==1.56.2 -grpc-google-iam-v1 +grpc-google-iam-v1==0.12.4 grpcio-status==1.46.3 grpcio==1.46.3 httplib2 From 7cfe72b5b2fdbb5f23988b540eceaaa91384095b Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:58:12 +0900 Subject: [PATCH 19/33] list all packages in jsk_3rdparty --- jsk_3rdparty/package.xml | 55 +++++++++++++++++++++++++++++----------- 1 file changed, 40 insertions(+), 15 deletions(-) diff --git a/jsk_3rdparty/package.xml b/jsk_3rdparty/package.xml index 74ed7afd8..0e546d244 100644 --- a/jsk_3rdparty/package.xml +++ b/jsk_3rdparty/package.xml @@ -16,28 +16,53 @@ catkin + bayesian_belief_networks + chaplus_ros + dialogflow_task_executive + gdrive_ros + google_chat_ros + hark_jsk_plugins + influxdb_store + jsk_database_scripts + julius_ros opt_camera - rosping pgm_learner + respeaker_ros + ros_google_cloud_language + ros_speech_recognition rospatlite - nlopt - voice_text - slic + rosping + rostwitter + sesame_ros + switchbot_ros + webrtcvad_ros + + + aques_talk assimp_devel + crlibm + downward ff - mini_maxwell - libcmt - libsiftfast ffha - downward - rostwitter - bayesian_belief_networks - julius - julius_ros - sesame_ros - dialogflow_task_executive - gdrive_ros google_cloud_texttospeech + jtalk + julius + libcmt + libsiftfast + lpg_planner + mini_maxwell + nlopt + python_twitter + python_twoauth + slic + voice_text + + + collada_urdf_jsk_patch + + eecart_imped_jsk_patch + laser_filters_jsk_patch + From 3cd35fc48e03df0807cd612e964eb131bb4035cb Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 31 Oct 2022 16:16:21 +0900 Subject: [PATCH 20/33] add zdepth package --- 3rdparty/zdepth/CMakeLists.txt | 17 ++++++++++++++ 3rdparty/zdepth/fix_cmakelists.patch | 34 ++++++++++++++++++++++++++++ 3rdparty/zdepth/package.xml | 16 +++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 3rdparty/zdepth/CMakeLists.txt create mode 100644 3rdparty/zdepth/fix_cmakelists.patch create mode 100644 3rdparty/zdepth/package.xml diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt new file mode 100644 index 000000000..5bd879795 --- /dev/null +++ b/3rdparty/zdepth/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zdepth) + +include(ExternalProject) + +ExternalProject_Add( + zdepth + GIT_REPOSITORY https://github.com/catid/Zdepth.git + GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 + GIT_SHALLOW TRUE + PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ + USE_SOURCE_PERMISSIONS) diff --git a/3rdparty/zdepth/fix_cmakelists.patch b/3rdparty/zdepth/fix_cmakelists.patch new file mode 100644 index 000000000..7ea697610 --- /dev/null +++ b/3rdparty/zdepth/fix_cmakelists.patch @@ -0,0 +1,34 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index b22b2e7..a589f4d 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -80,10 +80,13 @@ target_link_libraries(zdepth_test PRIVATE zdepth) + ################################################################################ + # Install + +-install(FILES LICENSE DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(FILES README.md DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(DIRECTORY cmake include DESTINATION ${CMAKE_INSTALL_PREFIX}) +-install(TARGETS zdepth EXPORT zdepth LIBRARY DESTINATION lib) ++# install(FILES LICENSE DESTINATION ${CMAKE_INSTALL_PREFIX}) ++# install(FILES README.md DESTINATION ${CMAKE_INSTALL_PREFIX}) ++install(FILES include/zdepth.hpp DESTINATION ${CMAKE_INSTALL_PREFIX}/include/zdepth) ++install(FILES tests/test_vectors.inl DESTINATION ${CMAKE_INSTALL_PREFIX}/include/zdepth/tests) ++install(TARGETS zdepth EXPORT zdepth ++ ARCHIVE DESTINATION lib ++ LIBRARY DESTINATION lib) + + ################################################################################ + # Generate zdepthConfig.cmake and zdepthConfigVersion.cmake for cmake projects +diff --git a/zstd/CMakeLists.txt b/zstd/CMakeLists.txt +index 5869d4d..c0aa503 100644 +--- a/zstd/CMakeLists.txt ++++ b/zstd/CMakeLists.txt +@@ -25,4 +25,6 @@ target_include_directories(zstd PUBLIC + "$" + ) + +-install(TARGETS zstd EXPORT zdepth LIBRARY DESTINATION lib) ++install(TARGETS zstd EXPORT zdepth ++ ARCHIVE DESTINATION lib ++ LIBRARY DESTINATION lib) diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml new file mode 100644 index 000000000..7f9107564 --- /dev/null +++ b/3rdparty/zdepth/package.xml @@ -0,0 +1,16 @@ + + + zdepth + 0.0.0 + The zdepth package + + Shingo Kitagawa + + BSD + + catkin + + + + + From 923f87d23ae0122e8c64fbcc8cd2a3be6bdce8bd Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:15:14 +0900 Subject: [PATCH 21/33] build zdepth as shared library --- 3rdparty/zdepth/fix_cmakelists.patch | 22 +++++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/3rdparty/zdepth/fix_cmakelists.patch b/3rdparty/zdepth/fix_cmakelists.patch index 7ea697610..cc5191852 100644 --- a/3rdparty/zdepth/fix_cmakelists.patch +++ b/3rdparty/zdepth/fix_cmakelists.patch @@ -1,7 +1,16 @@ diff --git a/CMakeLists.txt b/CMakeLists.txt -index b22b2e7..a589f4d 100644 +index b22b2e7..a4251af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt +@@ -61,7 +61,7 @@ include_directories(include) + + # zdepth library + +-add_library(zdepth STATIC ${SOURCE_FILES}) ++add_library(zdepth SHARED ${SOURCE_FILES}) + add_library(zdepth::zdepth ALIAS zdepth) + + target_link_libraries(zdepth PUBLIC zstd) @@ -80,10 +80,13 @@ target_link_libraries(zdepth_test PRIVATE zdepth) ################################################################################ # Install @@ -21,10 +30,17 @@ index b22b2e7..a589f4d 100644 ################################################################################ # Generate zdepthConfig.cmake and zdepthConfigVersion.cmake for cmake projects diff --git a/zstd/CMakeLists.txt b/zstd/CMakeLists.txt -index 5869d4d..c0aa503 100644 +index 5869d4d..f9339a3 100644 --- a/zstd/CMakeLists.txt +++ b/zstd/CMakeLists.txt -@@ -25,4 +25,6 @@ target_include_directories(zstd PUBLIC +@@ -19,10 +19,12 @@ include_directories(include src) + ################################################################################ + # Targets + +-add_library(zstd ${ZSTD_SOURCE_FILES}) ++add_library(zstd SHARED ${ZSTD_SOURCE_FILES}) + target_include_directories(zstd PUBLIC + "$" "$" ) From 61c14b7925f633795469070362e2f6132d3760b5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:29:15 +0900 Subject: [PATCH 22/33] update package information --- 3rdparty/zdepth/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml index 7f9107564..fe4d3cde8 100644 --- a/3rdparty/zdepth/package.xml +++ b/3rdparty/zdepth/package.xml @@ -4,7 +4,7 @@ 0.0.0 The zdepth package - Shingo Kitagawa + Shingo Kitagawa BSD From c917bb58ca37ea3ce8a7335ec1b5234df0dedd7d Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:29:22 +0900 Subject: [PATCH 23/33] add publisher plugin and zdepth_image_transport --- .../zdepth_image_transport/CMakeLists.txt | 71 ++++ .../zdepth_image_transport/zdepth_publisher.h | 22 ++ .../msg/ZDepthImage.msg | 15 + 3rdparty/zdepth_image_transport/package.xml | 31 ++ .../zdepth_image_transport/src/manifest.cpp | 6 + .../src/zdepth_publisher.cpp | 44 +++ .../src/zdepth_test.cpp | 368 ++++++++++++++++++ .../zdepth_image_transport/zdepth_plugins.xml | 15 + 8 files changed, 572 insertions(+) create mode 100644 3rdparty/zdepth_image_transport/CMakeLists.txt create mode 100644 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h create mode 100644 3rdparty/zdepth_image_transport/msg/ZDepthImage.msg create mode 100644 3rdparty/zdepth_image_transport/package.xml create mode 100644 3rdparty/zdepth_image_transport/src/manifest.cpp create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_test.cpp create mode 100644 3rdparty/zdepth_image_transport/zdepth_plugins.xml diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt new file mode 100644 index 000000000..90ed3ea85 --- /dev/null +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 2.8.3) +project(zdepth_image_transport) + +find_package(catkin REQUIRED + COMPONENTS + image_transport + message_generation + pluginlib + roscpp + sensor_msgs + std_msgs +) + +find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) +find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) +set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) +find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) +get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) +get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) + +add_message_files( + DIRECTORY msg + FILES + ZDepthImage.msg +) + +generate_messages(DEPENDENCIES std_msgs) + + +catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS + message_runtime + image_transport + pluginlib + roscpp + sensor_msgs + std_msgs + DEPENDS + zdepth +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) + +add_executable(zdepth_test src/zdepth_test.cpp) +target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + +# add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) +add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + +install(TARGETS zdepth_test + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES zdepth_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h new file mode 100644 index 000000000..4c86c5215 --- /dev/null +++ b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h @@ -0,0 +1,22 @@ +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/ZDepthImage.h" + + +namespace zdepth_image_transport +{ + class ZDepthPublisher : public image_transport::SimplePublisherPlugin + { + public: + virtual ~ZDepthPublisher() {} + virtual std::string getTransportName() const + { + return "zdepth"; + } + + protected: + virtual void publish(const sensor_msgs::Image& img_msg, + const PublishFn& publish_fn) const; + }; + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg b/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg new file mode 100644 index 000000000..8cb12028e --- /dev/null +++ b/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg @@ -0,0 +1,15 @@ +# This message contains a compressed image + +Header header # Header timestamp should be acquisition time of image + # Header frame_id should be optical frame of camera + # origin of frame should be optical center of camera + # +x should point to the right in the image + # +y should point down in the image + # +z should point into to plane of the image + +string format # Specifies the format of the data + # Acceptable values: + # jpeg, png +uint8[] data # Compressed image buffer +int32 width +int32 height diff --git a/3rdparty/zdepth_image_transport/package.xml b/3rdparty/zdepth_image_transport/package.xml new file mode 100644 index 000000000..7a2d2cbd6 --- /dev/null +++ b/3rdparty/zdepth_image_transport/package.xml @@ -0,0 +1,31 @@ + + + zdepth_image_transport + 0.0.0 + The zdepth_image_transport package + + Shingo Kitagawa + + BSD + + + catkin + image_transport + message_generation + pluginlib + roscpp + sensor_msgs + std_msgs + zdepth + + image_transport + message_runtime + pluginlib + roscpp + sensor_msgs + std_msgs + zdepth + + + + diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/3rdparty/zdepth_image_transport/src/manifest.cpp new file mode 100644 index 000000000..fdb5a02ef --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/manifest.cpp @@ -0,0 +1,6 @@ +#include +#include +// #include + +PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthPublisher, image_transport::PublisherPlugin) +// PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp new file mode 100644 index 000000000..3502ccc79 --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp @@ -0,0 +1,44 @@ +// #include + +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/zdepth_publisher.h" + +#include +#include "zdepth_image_transport/ZDepthImage.h" + +namespace zdepth_image_transport +{ + + void ZDepthPublisher::publish(const sensor_msgs::Image& img_msg, + const PublishFn& publish_fn) const + { + if (img_msg.encoding != "16UC1") + { + ROS_ERROR("Unsupported encoding: %s", img_msg.encoding.c_str()); + return; + } + + zdepth::DepthCompressor compressor; + std::vector compressed; + int width = img_msg.width; + int height = img_msg.height; + + const uint16_t* frame = (uint16_t*)img_msg.data.data(); + zdepth::DepthResult result = compressor.Compress(width, height, frame, compressed, true); + if (result != zdepth::DepthResult::Success) { + ROS_ERROR("Failed to compress"); + return; + } + + // publish + zdepth_image_transport::ZDepthImage::Ptr compressed_img_msg(new zdepth_image_transport::ZDepthImage()); + compressed_img_msg->header = img_msg.header; + compressed_img_msg->data = compressed; + compressed_img_msg->format = "16UC1; zdepth"; + compressed_img_msg->width = width; + compressed_img_msg->height = height; + publish_fn(*compressed_img_msg); + } + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/src/zdepth_test.cpp b/3rdparty/zdepth_image_transport/src/zdepth_test.cpp new file mode 100644 index 000000000..5d2babf7f --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_test.cpp @@ -0,0 +1,368 @@ +// Copyright 2019 (c) Christopher A. Taylor. All rights reserved. + +#include "zdepth/zdepth.hpp" +using namespace zdepth; + + +//------------------------------------------------------------------------------ +// Timing + +#ifdef _WIN32 + #ifndef NOMINMAX + #define NOMINMAX + #endif + #include +#elif __MACH__ + #include + #include + #include + #include + + extern mach_port_t clock_port; +#else + #include + #include + #include // flock +#endif + +#ifdef _WIN32 +// Precomputed frequency inverse +static double PerfFrequencyInverseUsec = 0.; +static double PerfFrequencyInverseMsec = 0.; + +static void InitPerfFrequencyInverse() +{ + LARGE_INTEGER freq = {}; + if (!::QueryPerformanceFrequency(&freq) || freq.QuadPart == 0) { + return; + } + const double invFreq = 1. / (double)freq.QuadPart; + PerfFrequencyInverseUsec = 1000000. * invFreq; + PerfFrequencyInverseMsec = 1000. * invFreq; +} +#elif __MACH__ +static bool m_clock_serv_init = false; +static clock_serv_t m_clock_serv = 0; + +static void InitClockServ() +{ + m_clock_serv_init = true; + host_get_clock_service(mach_host_self(), SYSTEM_CLOCK, &m_clock_serv); +} +#endif // _WIN32 + +uint64_t GetTimeUsec() +{ +#ifdef _WIN32 + LARGE_INTEGER timeStamp = {}; + if (!::QueryPerformanceCounter(&timeStamp)) { + return 0; + } + if (PerfFrequencyInverseUsec == 0.) { + InitPerfFrequencyInverse(); + } + return (uint64_t)(PerfFrequencyInverseUsec * timeStamp.QuadPart); +#elif __MACH__ + if (!m_clock_serv_init) { + InitClockServ(); + } + + mach_timespec_t tv; + clock_get_time(m_clock_serv, &tv); + + return 1000000 * tv.tv_sec + tv.tv_nsec / 1000; +#else + // This seems to be the best clock to used based on: + // http://btorpey.github.io/blog/2014/02/18/clock-sources-in-linux/ + // The CLOCK_MONOTONIC_RAW seems to take a long time to query, + // and so it only seems useful for timing code that runs a small number of times. + // The CLOCK_MONOTONIC is affected by NTP at 500ppm but doesn't make sudden jumps. + // Applications should already be robust to clock skew so this is acceptable. + struct timespec ts; + clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_nsec / 1000) + static_cast(ts.tv_sec) * 1000000; +#endif +} + + +//------------------------------------------------------------------------------ +// RVL + +// RVL library for performance baseline + +// Paper: https://www.microsoft.com/en-us/research/publication/fast-lossless-depth-image-compression/ +// Video presentation: https://www.youtube.com/watch?v=WYU2upBs2hA + +// RVL author suggests that H.264 is a bad idea to use. +// But it seems like some masking can be used to avoid messing up the edges... + +// Effective Compression of Range Data Streams for Remote Robot Operations using H.264 +// http://www2.informatik.uni-freiburg.de/~stachnis/pdf/nenci14iros.pdf + +// Adapting Standard Video Codecs for Depth Streaming +// http://reality.cs.ucl.ac.uk/projects/depth-streaming/depth-streaming.pdf + +inline void EncodeVLE(int* &pBuffer, int& word, int& nibblesWritten, int value) +{ + do + { + int nibble = value & 0x7; // lower 3 bits + if (value >>= 3) { + nibble |= 0x8; // more to come + } + word <<= 4; + word |= nibble; + if (++nibblesWritten == 8) // output word + { + *pBuffer++ = word; + nibblesWritten = 0; + word = 0; + } + } while (value); +} + +inline int DecodeVLE(int* &pBuffer, int& word, int& nibblesWritten) +{ + unsigned int nibble; + int value = 0, bits = 29; + do + { + if (!nibblesWritten) + { + word = *pBuffer++; // load word + nibblesWritten = 8; + } + nibble = word & 0xf0000000; + value |= (nibble << 1) >> bits; + word <<= 4; + nibblesWritten--; + bits -= 3; + } while (nibble & 0x80000000); + return value; +} + +int CompressRVL(short* input, char* output, int numPixels) +{ + int word{ 0 }; + int nibblesWritten; + int *pBuffer; + int *buffer = pBuffer = (int*)output; + nibblesWritten = 0; + short *end = input + numPixels; + short previous = 0; + while (input != end) + { + int zeros = 0, nonzeros = 0; + for (; (input != end) && !*input; input++, zeros++); + EncodeVLE(pBuffer, word, nibblesWritten, zeros); // number of zeros + for (short* p = input; (p != end) && *p++; nonzeros++); + EncodeVLE(pBuffer, word, nibblesWritten, nonzeros); // number of nonzeros + for (int i = 0; i < nonzeros; i++) + { + short current = *input++; + int delta = current - previous; + int positive = (delta << 1) ^ (delta >> 31); + EncodeVLE(pBuffer, word, nibblesWritten, positive); // nonzero value + previous = current; + } + } + if (nibblesWritten) // last few values + { + *pBuffer++ = word << 4 * (8 - nibblesWritten); + } + return int((char*)pBuffer - (char*)buffer); // num bytes +} + +void DecompressRVL(char* input, short* output, int numPixels) +{ + int word, nibblesWritten; + int *pBuffer = (int*)input; + nibblesWritten = 0; + short current, previous = 0; + int numPixelsToDecode = numPixels; + while (numPixelsToDecode) + { + int zeros = DecodeVLE(pBuffer, word, nibblesWritten); // number of zeros + numPixelsToDecode -= zeros; + for (; zeros; zeros--) { + *output++ = 0; + } + int nonzeros = DecodeVLE(pBuffer, word, nibblesWritten); // number of nonzeros + numPixelsToDecode -= nonzeros; + for (; nonzeros; nonzeros--) + { + int positive = DecodeVLE(pBuffer, word, nibblesWritten); // nonzero value + int delta = (positive >> 1) ^ -(positive & 1); + current = previous + static_cast(delta); + *output++ = current; + previous = current; + } + } +} + + +//------------------------------------------------------------------------------ +// Test Vectors + +#include "zdepth/tests/test_vectors.inl" + + +//------------------------------------------------------------------------------ +// Test Application + +#include +using namespace std; + +static zdepth::DepthCompressor compressor, decompressor; + +bool TestFrame(const uint16_t* frame, bool keyframe) +{ + std::vector compressed; + + static_assert(Width % kBlockSize == 0, "Width is not a multiple of block size."); + static_assert(Height % kBlockSize == 0, "Height is not a multiple of block size."); + + const uint64_t t0 = GetTimeUsec(); + + compressor.Compress(Width, Height, frame, compressed, keyframe); + + const uint64_t t1 = GetTimeUsec(); + + int width, height; + std::vector depth; + zdepth::DepthResult result = decompressor.Decompress(compressed, width, height, depth); + + const uint64_t t2 = GetTimeUsec(); + + if (result != zdepth::DepthResult::Success) { + cout << "Failed: decompressor.Decompress returned " << zdepth::DepthResultString(result) << endl; + return false; + } + if (width != Width || + height != Height) + { + cout << "Decompression failed: Resolution" << endl; + return false; + } + + for (int i = 0; i < depth.size(); ++i) { + if (AzureKinectQuantizeDepth(depth[i]) != AzureKinectQuantizeDepth(frame[i])) { + cout << "Decompression failed: Contents did not match at offset = " << i << endl; + return false; + } + } + + const unsigned original_bytes = Width * Height * 2; + cout << endl; + cout << "Zdepth Compression: " << original_bytes << " bytes -> " << compressed.size() << + " bytes (ratio = " << original_bytes / (float)compressed.size() << ":1) (" + << (compressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Zdepth Speed: Compressed in " << (t1 - t0) / 1000.f << " msec. Decompressed in " << (t2 - t1) / 1000.f << " msec" << endl; + + const int n = Width * Height; + std::vector quantized(n); + compressed.resize(n * 3); + + const uint64_t t3 = GetTimeUsec(); + QuantizeDepthImage(Width, Height, frame, quantized); + const int compressed_bytes = CompressRVL((short*)quantized.data(), (char*)compressed.data(), n); + compressed.resize(compressed_bytes); + const uint64_t t4 = GetTimeUsec(); + + std::vector recompressed; + std::vector decompressed; + + const uint64_t t5 = GetTimeUsec(); + ZstdCompress(compressed, recompressed); + const uint64_t t6 = GetTimeUsec(); + ZstdDecompress(recompressed.data(), static_cast(recompressed.size()), static_cast(compressed.size()), decompressed); + const uint64_t t7 = GetTimeUsec(); + quantized.resize(n * 2); + DecompressRVL((char*)decompressed.data(), (short*)quantized.data(), n); + DequantizeDepthImage(Width, Height, quantized.data(), depth); + const uint64_t t8 = GetTimeUsec(); + + for (int i = 0; i < depth.size(); ++i) { + if (AzureKinectQuantizeDepth(depth[i]) != AzureKinectQuantizeDepth(frame[i])) { + cout << "Decompression failed: Contents did not match at offset = " << i << endl; + return false; + } + } + + cout << endl; + cout << "Quantization+RVL Compression: " << original_bytes << " bytes -> " << compressed.size() << + " bytes (ratio = " << original_bytes / (float)compressed.size() << ":1) (" + << (compressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Quantization+RVL Speed: Compressed in " << (t4 - t3) / 1000.f << " msec. Decompressed in " << (t8 - t7) / 1000.f << " msec" << endl; + + cout << endl; + cout << "Quantization+RVL+Zstd Compression: " << original_bytes << " bytes -> " << recompressed.size() << + " bytes (ratio = " << original_bytes / (float)recompressed.size() << ":1) (" + << (recompressed.size() * 30 * 8) / 1000000.f << " Mbps @ 30 FPS)" << endl; + cout << "Quantization+RVL+Zstd Speed: Compressed in " << (t6 - t5 + t4 - t3) / 1000.f << " msec. Decompressed in " << (t8 - t6) / 1000.f << " msec" << endl; + + return true; +} + +bool TestPattern(const uint16_t* frame0, const uint16_t* frame1) +{ + cout << endl; + cout << "===================================================================" << endl; + cout << "+ Test: Frame 0 Keyframe=true compression" << endl; + cout << "===================================================================" << endl; + + if (!TestFrame(frame0, true)) { + cout << "Failure: frame0 failed"; + return false; + } + + cout << endl; + cout << "===================================================================" << endl; + cout << "+ Test: Frame 1 Keyframe=false compression" << endl; + cout << "===================================================================" << endl; + + if (!TestFrame(frame1, false)) { + cout << "Failure: frame1 failed"; + return false; + } + return true; +} + +int main(int argc, char* argv[]) +{ + (void)(argc); (void)(argv); + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Room" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector0_Room0, TestVector0_Room1)) { + cout << "Test failure: Room test vector" << endl; + return -1; + } + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Ceiling" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector1_Ceiling0, TestVector1_Ceiling1)) { + cout << "Test failure: Ceiling test vector" << endl; + return -2; + } + + cout << endl; + cout << "-------------------------------------------------------------------" << endl; + cout << "Test vector: Person" << endl; + cout << "-------------------------------------------------------------------" << endl; + + if (!TestPattern(TestVector2_Person0, TestVector2_Person1)) { + cout << "Test failure: Person test vector" << endl; + return -3; + } + cout << endl; + + cout << "Test success" << endl; + return 0; +} diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/3rdparty/zdepth_image_transport/zdepth_plugins.xml new file mode 100644 index 000000000..400bedaf8 --- /dev/null +++ b/3rdparty/zdepth_image_transport/zdepth_plugins.xml @@ -0,0 +1,15 @@ + + + + This plugin publishes a zdepth version of the image. + + + + + From d909a1f1e93032b2238a307771a49653ed5e2ce7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 2 Nov 2022 23:48:00 +0900 Subject: [PATCH 24/33] add zdepth subsriber plugin --- .../zdepth_image_transport/CMakeLists.txt | 3 +- .../zdepth_subscriber.h | 22 ++++++++ .../zdepth_image_transport/src/manifest.cpp | 4 +- .../src/zdepth_subscriber.cpp | 54 +++++++++++++++++++ .../zdepth_image_transport/zdepth_plugins.xml | 2 - 5 files changed, 79 insertions(+), 6 deletions(-) create mode 100644 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h create mode 100644 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 90ed3ea85..6441083f7 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -46,8 +46,7 @@ include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) add_executable(zdepth_test src/zdepth_test.cpp) target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -# add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) -add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp) +add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) install(TARGETS zdepth_test diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h new file mode 100644 index 000000000..00f348c87 --- /dev/null +++ b/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h @@ -0,0 +1,22 @@ +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/ZDepthImage.h" + + +namespace zdepth_image_transport +{ + class ZDepthSubscriber : public image_transport::SimpleSubscriberPlugin + { + public: + virtual ~ZDepthSubscriber() {} + virtual std::string getTransportName() const + { + return "zdepth"; + } + + protected: + virtual void internalCallback(const zdepth_image_transport::ZDepthImageConstPtr& zdepth_msg, + const Callback& user_cb); + }; + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/3rdparty/zdepth_image_transport/src/manifest.cpp index fdb5a02ef..5075e5a2e 100644 --- a/3rdparty/zdepth_image_transport/src/manifest.cpp +++ b/3rdparty/zdepth_image_transport/src/manifest.cpp @@ -1,6 +1,6 @@ #include #include -// #include +#include PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthPublisher, image_transport::PublisherPlugin) -// PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS(zdepth_image_transport::ZDepthSubscriber, image_transport::SubscriberPlugin) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp new file mode 100644 index 000000000..8f01bd7bf --- /dev/null +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -0,0 +1,54 @@ +// #include + +#include +#include "zdepth/zdepth.hpp" +#include "zdepth_image_transport/zdepth_subscriber.h" + +#include +#include "zdepth_image_transport/ZDepthImage.h" + +namespace zdepth_image_transport +{ + void ZDepthSubscriber::internalCallback(const zdepth_image_transport::ZDepthImageConstPtr& zdepth_msg, + const Callback& user_cb) + { + if (zdepth_msg->format != "16UC1; zdepth") + { + ROS_ERROR("Unsupported format: %s", zdepth_msg->format.c_str()); + return; + } + + zdepth::DepthCompressor decompressor; + std::vector decompressed; + int width = zdepth_msg->width; + int height = zdepth_msg->height; + + std::vector depth; + zdepth::DepthResult result = decompressor.Decompress( + zdepth_msg->data, width, height, depth + ); + + if (result != zdepth::DepthResult::Success) { + ROS_ERROR("Failed to decompress"); + return; + } + + std::vector depth_vec; + for (int i=0; i(&depth[i]); + std::vector dv(d, d+2); + depth_vec.insert(depth_vec.end(), dv.begin(), dv.end()); + } + + // callback + sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); + img_msg->header = zdepth_msg->header; + img_msg->data = depth_vec; + img_msg->width = width; + img_msg->height = height; + img_msg->encoding = "16UC1"; + img_msg->step = width * 2; + user_cb(img_msg); + } + +} //namespace zdepth_image_transport diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/3rdparty/zdepth_image_transport/zdepth_plugins.xml index 400bedaf8..b9de359a9 100644 --- a/3rdparty/zdepth_image_transport/zdepth_plugins.xml +++ b/3rdparty/zdepth_image_transport/zdepth_plugins.xml @@ -5,11 +5,9 @@ - From 92ae83c8a8719fa9a415e08a0b1a71d05fbb6ac9 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 3 Nov 2022 02:05:38 +0900 Subject: [PATCH 25/33] make decompress copy faster --- .../src/zdepth_subscriber.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp index 8f01bd7bf..baed84c15 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -1,5 +1,6 @@ // #include +#include #include #include "zdepth/zdepth.hpp" #include "zdepth_image_transport/zdepth_subscriber.h" @@ -23,22 +24,26 @@ namespace zdepth_image_transport int width = zdepth_msg->width; int height = zdepth_msg->height; + auto start_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); std::vector depth; zdepth::DepthResult result = decompressor.Decompress( zdepth_msg->data, width, height, depth ); + auto decompress_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); if (result != zdepth::DepthResult::Success) { ROS_ERROR("Failed to decompress"); return; } - std::vector depth_vec; - for (int i=0; i(&depth[i]); - std::vector dv(d, d+2); - depth_vec.insert(depth_vec.end(), dv.begin(), dv.end()); - } + uint8_t* depth_vec_beg = reinterpret_cast(&depth[0]); + uint8_t* depth_vec_end = depth_vec_beg + depth.size() * 2; + std::vector depth_vec(depth_vec_beg, depth_vec_end); + auto copy_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + + // DEBUG + ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); + ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); // callback sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); From 050ff04bd148af4955d61a27edaadb2cbb3ea96c Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 3 Nov 2022 02:19:21 +0900 Subject: [PATCH 26/33] move zdepth_test.cpp --- 3rdparty/zdepth_image_transport/CMakeLists.txt | 2 +- 3rdparty/zdepth_image_transport/src/{ => test}/zdepth_test.cpp | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename 3rdparty/zdepth_image_transport/src/{ => test}/zdepth_test.cpp (100%) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 6441083f7..1824acee8 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -43,7 +43,7 @@ catkin_package( include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) -add_executable(zdepth_test src/zdepth_test.cpp) +add_executable(zdepth_test src/test/zdepth_test.cpp) target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_test.cpp b/3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_test.cpp rename to 3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp From e982668294d12a0d32f923234567e574ce0e3c59 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Fri, 16 Dec 2022 20:36:20 +0900 Subject: [PATCH 27/33] fix dependencies in zdepth_image_transport --- 3rdparty/zdepth_image_transport/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/3rdparty/zdepth_image_transport/CMakeLists.txt index 1824acee8..227f6cf77 100644 --- a/3rdparty/zdepth_image_transport/CMakeLists.txt +++ b/3rdparty/zdepth_image_transport/CMakeLists.txt @@ -48,6 +48,7 @@ target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) +add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) install(TARGETS zdepth_test ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} From f5095cb20ea1c15f3e60da12c940e6c42fb020b7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Mon, 19 Dec 2022 18:09:58 +0900 Subject: [PATCH 28/33] fix zdepth package --- 3rdparty/zdepth/CMakeLists.txt | 8 ++++---- 3rdparty/zdepth/package.xml | 1 + 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt index 5bd879795..392eed536 100644 --- a/3rdparty/zdepth/CMakeLists.txt +++ b/3rdparty/zdepth/CMakeLists.txt @@ -9,9 +9,9 @@ ExternalProject_Add( GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 GIT_SHALLOW TRUE PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 - CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INSTALL_COMMAND echo "install" ) -install(DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ - USE_SOURCE_PERMISSIONS) + +install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") diff --git a/3rdparty/zdepth/package.xml b/3rdparty/zdepth/package.xml index fe4d3cde8..c4b511c4d 100644 --- a/3rdparty/zdepth/package.xml +++ b/3rdparty/zdepth/package.xml @@ -12,5 +12,6 @@ + cmake From 20fb79bdb73ce808d8c181447560dc428f072874 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:45:10 +0900 Subject: [PATCH 29/33] comment out debug outputs --- 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp index baed84c15..c7031cf72 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp @@ -42,8 +42,8 @@ namespace zdepth_image_transport auto copy_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); // DEBUG - ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); - ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); + // ROS_DEBUG_STREAM("decompress: " << decompress_time - start_time << "ms"); + // ROS_DEBUG_STREAM("copy : " << copy_time - decompress_time << "ms"); // callback sensor_msgs::Image::Ptr img_msg(new sensor_msgs::Image()); From 7ad329c4f0e72571d709f1e4888b3ba989aebb91 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:45:26 +0900 Subject: [PATCH 30/33] support 32fc1 --- .../src/zdepth_publisher.cpp | 23 ++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp index 3502ccc79..32bbbb918 100644 --- a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp +++ b/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp @@ -13,7 +13,7 @@ namespace zdepth_image_transport void ZDepthPublisher::publish(const sensor_msgs::Image& img_msg, const PublishFn& publish_fn) const { - if (img_msg.encoding != "16UC1") + if (img_msg.encoding != "16UC1" && img_msg.encoding != "32FC1") { ROS_ERROR("Unsupported encoding: %s", img_msg.encoding.c_str()); return; @@ -23,9 +23,26 @@ namespace zdepth_image_transport std::vector compressed; int width = img_msg.width; int height = img_msg.height; + zdepth::DepthResult result; + + if (img_msg.encoding == "16UC1") + { + const uint16_t* frame = (uint16_t*)img_msg.data.data(); + result = compressor.Compress(width, height, frame, compressed, true); + } + else + { + // convert 32fc1 -> 16uc1 + const float* frame_32fc1 = reinterpret_cast(img_msg.data.data()); + uint16_t* frame_16uc1 = new uint16_t[height*width](); + for (unsigned index = 0; index < height * width; ++index) + { + float d = frame_32fc1[index]; + frame_16uc1[index] = std::isnan(d) ? 0 : (uint16_t)(d * 1000); + } + result = compressor.Compress(width, height, frame_16uc1, compressed, true); + } - const uint16_t* frame = (uint16_t*)img_msg.data.data(); - zdepth::DepthResult result = compressor.Compress(width, height, frame, compressed, true); if (result != zdepth::DepthResult::Success) { ROS_ERROR("Failed to compress"); return; From 7003d1ea5a0f5a5d5bf5d527cf9d7cd335d833b7 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 00:46:04 +0900 Subject: [PATCH 31/33] add zdepth,zdepth_image_transport in jsk_3rdparty depends --- jsk_3rdparty/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/jsk_3rdparty/package.xml b/jsk_3rdparty/package.xml index 0e546d244..ccdb96994 100644 --- a/jsk_3rdparty/package.xml +++ b/jsk_3rdparty/package.xml @@ -36,6 +36,7 @@ sesame_ros switchbot_ros webrtcvad_ros + zdepth_image_transport aques_talk @@ -56,6 +57,7 @@ python_twoauth slic voice_text + zdepth collada_urdf_jsk_patch From d8556146e8a57fc7d4ed09b9843b86e1cc4ca8e5 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 01:00:10 +0900 Subject: [PATCH 32/33] mv zdepth_image_transport from 3rdparty/ to / --- .../CMakeLists.txt | 0 .../include/zdepth_image_transport/zdepth_publisher.h | 0 .../include/zdepth_image_transport/zdepth_subscriber.h | 0 .../msg/ZDepthImage.msg | 0 .../zdepth_image_transport => zdepth_image_transport}/package.xml | 0 .../src/manifest.cpp | 0 .../src/test/zdepth_test.cpp | 0 .../src/zdepth_publisher.cpp | 0 .../src/zdepth_subscriber.cpp | 0 .../zdepth_plugins.xml | 0 10 files changed, 0 insertions(+), 0 deletions(-) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/CMakeLists.txt (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/include/zdepth_image_transport/zdepth_publisher.h (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/include/zdepth_image_transport/zdepth_subscriber.h (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/msg/ZDepthImage.msg (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/package.xml (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/manifest.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/test/zdepth_test.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/zdepth_publisher.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/src/zdepth_subscriber.cpp (100%) rename {3rdparty/zdepth_image_transport => zdepth_image_transport}/zdepth_plugins.xml (100%) diff --git a/3rdparty/zdepth_image_transport/CMakeLists.txt b/zdepth_image_transport/CMakeLists.txt similarity index 100% rename from 3rdparty/zdepth_image_transport/CMakeLists.txt rename to zdepth_image_transport/CMakeLists.txt diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h b/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h similarity index 100% rename from 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h rename to zdepth_image_transport/include/zdepth_image_transport/zdepth_publisher.h diff --git a/3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h b/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h similarity index 100% rename from 3rdparty/zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h rename to zdepth_image_transport/include/zdepth_image_transport/zdepth_subscriber.h diff --git a/3rdparty/zdepth_image_transport/msg/ZDepthImage.msg b/zdepth_image_transport/msg/ZDepthImage.msg similarity index 100% rename from 3rdparty/zdepth_image_transport/msg/ZDepthImage.msg rename to zdepth_image_transport/msg/ZDepthImage.msg diff --git a/3rdparty/zdepth_image_transport/package.xml b/zdepth_image_transport/package.xml similarity index 100% rename from 3rdparty/zdepth_image_transport/package.xml rename to zdepth_image_transport/package.xml diff --git a/3rdparty/zdepth_image_transport/src/manifest.cpp b/zdepth_image_transport/src/manifest.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/manifest.cpp rename to zdepth_image_transport/src/manifest.cpp diff --git a/3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp b/zdepth_image_transport/src/test/zdepth_test.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/test/zdepth_test.cpp rename to zdepth_image_transport/src/test/zdepth_test.cpp diff --git a/3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp b/zdepth_image_transport/src/zdepth_publisher.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_publisher.cpp rename to zdepth_image_transport/src/zdepth_publisher.cpp diff --git a/3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp b/zdepth_image_transport/src/zdepth_subscriber.cpp similarity index 100% rename from 3rdparty/zdepth_image_transport/src/zdepth_subscriber.cpp rename to zdepth_image_transport/src/zdepth_subscriber.cpp diff --git a/3rdparty/zdepth_image_transport/zdepth_plugins.xml b/zdepth_image_transport/zdepth_plugins.xml similarity index 100% rename from 3rdparty/zdepth_image_transport/zdepth_plugins.xml rename to zdepth_image_transport/zdepth_plugins.xml From 4824c7caf309a61e40290ca6a1dc6ae110a0caa9 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Thu, 26 Jan 2023 19:53:11 +0900 Subject: [PATCH 33/33] zdepth skip building kinetic --- 3rdparty/zdepth/CMakeLists.txt | 25 ++++++++------ zdepth_image_transport/CMakeLists.txt | 48 +++++++++++++++------------ 2 files changed, 40 insertions(+), 33 deletions(-) diff --git a/3rdparty/zdepth/CMakeLists.txt b/3rdparty/zdepth/CMakeLists.txt index 392eed536..52bb0d590 100644 --- a/3rdparty/zdepth/CMakeLists.txt +++ b/3rdparty/zdepth/CMakeLists.txt @@ -3,15 +3,18 @@ project(zdepth) include(ExternalProject) -ExternalProject_Add( - zdepth - GIT_REPOSITORY https://github.com/catid/Zdepth.git - GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 - GIT_SHALLOW TRUE - PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 - CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} - INSTALL_COMMAND echo "install" - ) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + ExternalProject_Add( + zdepth + GIT_REPOSITORY https://github.com/catid/Zdepth.git + GIT_TAG ac7c6d8e944d07be2404e5a1eaa04562595f3756 + GIT_SHALLOW TRUE + PATCH_COMMAND cat ${PROJECT_SOURCE_DIR}/fix_cmakelists.patch | patch -p1 + CMAKE_ARGS -DCMAKE_BUILD_TYPE=${CMAKE_BUILD_TYPE} -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INSTALL_COMMAND echo "install" + ) - -install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") + install(CODE "execute_process(COMMAND make install WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/zdepth-prefix/src/zdepth-build)") +else() + install(CODE "message(WARNING \"Skipping zdepth install because ROS is too old\")") +endif() diff --git a/zdepth_image_transport/CMakeLists.txt b/zdepth_image_transport/CMakeLists.txt index 227f6cf77..beea8481b 100644 --- a/zdepth_image_transport/CMakeLists.txt +++ b/zdepth_image_transport/CMakeLists.txt @@ -11,12 +11,14 @@ find_package(catkin REQUIRED std_msgs ) -find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) -find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) -set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) -find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) -get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) -get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + find_library(zdepth_LIB zdepth PATH_SUFFIXES zdepth) + find_library(zstd_LIB zstd PATH_SUFFIXES zdepth) + set(zdepth_LIBRARIES ${zdepth_LIB} ${zstd_LIB}) + find_file(zdepth_INCLUDE zdepth.hpp PATH_SUFFIXES zdepth) + get_filename_component(zdepth_PATH ${zdepth_INCLUDE} DIRECTORY) + get_filename_component(zdepth_INCLUDE_DIRS ${zdepth_PATH} DIRECTORY) +endif() add_message_files( DIRECTORY msg @@ -41,26 +43,28 @@ catkin_package( zdepth ) -include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) +if("$ENV{ROS_DISTRO}" STRGREATER "kinetic") + include_directories(include ${catkin_INCLUDE_DIRS} ${zdepth_INCLUDE_DIRS}) -add_executable(zdepth_test src/test/zdepth_test.cpp) -target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + add_executable(zdepth_test src/test/zdepth_test.cpp) + target_link_libraries(zdepth_test ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) + add_library(${PROJECT_NAME} src/manifest.cpp src/zdepth_publisher.cpp src/zdepth_subscriber.cpp) + target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${zdepth_LIBRARIES}) + add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_generate_messages_cpp) -install(TARGETS zdepth_test - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + install(TARGETS zdepth_test + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) +endif() install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}