diff --git a/README.md b/README.md
index e01f76c8..86a15cb0 100644
--- a/README.md
+++ b/README.md
@@ -5,8 +5,8 @@ Furthermore, this framework provides several interfaces that allow humans to int
# Requirements
-* ROS2 Foxy
-* Ubuntu 20.04
+* ROS2 Humble Hawksbill
+* Ubuntu 22.04
* Python 3
# Cite
diff --git a/ros_pybullet_rqt_plugin/package.xml b/ros_pybullet_rqt_plugin/package.xml
new file mode 100644
index 00000000..8a3ee078
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/package.xml
@@ -0,0 +1,30 @@
+
+
+
+ ros_pybullet_rqt_plugin
+ 1.0.0
+ ROS2-based RQT plugin for ROS-PyBullet Interface.
+ Jan Kaniuka
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ python_qt_binding
+ rqt_gui
+ rqt_gui_py
+ rqt_py_common
+ rclpy
+ std_msgs
+
+ ros_pybullet_msgs
+ std_srvs
+
+
+
+ ament_python
+
+
+
diff --git a/ros_pybullet_rqt_plugin/plugin.xml b/ros_pybullet_rqt_plugin/plugin.xml
new file mode 100644
index 00000000..3ce2069e
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/plugin.xml
@@ -0,0 +1,17 @@
+
+
+
+ RQT plugin used to perform basic interactions with ROS-PyBullet Interface.
+
+
+
+
+ folder
+ Plugins related to introspection.
+
+
+ input-gaming
+ A Python GUI plugin for ROS-PyBullet Interface.
+
+
+
diff --git a/ros_pybullet_rqt_plugin/resource/PyBulletInterfaceControls_layout.ui b/ros_pybullet_rqt_plugin/resource/PyBulletInterfaceControls_layout.ui
new file mode 100644
index 00000000..718dc0a7
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/resource/PyBulletInterfaceControls_layout.ui
@@ -0,0 +1,207 @@
+
+
+ ROS
+
+
+
+ 0
+ 0
+ 560
+ 320
+
+
+
+
+ 560
+ 320
+
+
+
+ ROS
+
+
+ -
+
+
-
+
+
-
+
+
+
+ 0
+ 0
+
+
+
+
+
+
+ images/pybullet_logo.svg
+
+
+ true
+
+
+
+ -
+
+
+
+ 16
+
+
+
+ Qt::LeftToRight
+
+
+ background-color: rgb(255, 255, 255);
+
+
+ Initialization
+
+
+ Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop
+
+
+
+
+
+
+
+ -
+
+
-
+
+
-
+
+
+
+ 0
+ 0
+
+
+
+
+ 16
+
+
+
+ Qt::RightToLeft
+
+
+ Start
+
+
+
+ images/start_icon.pngimages/start_icon.png
+
+
+
+ 20
+ 20
+
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 16
+
+
+
+ Qt::RightToLeft
+
+
+ Send
+
+
+
+ images/send_icon.pngimages/send_icon.png
+
+
+
+ 20
+ 20
+
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 16
+
+
+
+ Qt::RightToLeft
+
+
+ Step
+
+
+
+ images/step_icon.pngimages/step_icon.png
+
+
+
+ 24
+ 23
+
+
+
+
+ -
+
+
+
+ 0
+ 0
+
+
+
+
+ 16
+
+
+
+ Qt::RightToLeft
+
+
+ Stop
+
+
+
+ images/stop_icon.pngimages/stop_icon.png
+
+
+
+ 20
+ 20
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros_pybullet_rqt_plugin/resource/images/pybullet_logo.svg b/ros_pybullet_rqt_plugin/resource/images/pybullet_logo.svg
new file mode 100644
index 00000000..67423e7a
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/resource/images/pybullet_logo.svg
@@ -0,0 +1,11 @@
+
\ No newline at end of file
diff --git a/ros_pybullet_rqt_plugin/resource/images/send_icon.png b/ros_pybullet_rqt_plugin/resource/images/send_icon.png
new file mode 100644
index 00000000..0a6429e2
Binary files /dev/null and b/ros_pybullet_rqt_plugin/resource/images/send_icon.png differ
diff --git a/ros_pybullet_rqt_plugin/resource/images/start_icon.png b/ros_pybullet_rqt_plugin/resource/images/start_icon.png
new file mode 100644
index 00000000..25a53779
Binary files /dev/null and b/ros_pybullet_rqt_plugin/resource/images/start_icon.png differ
diff --git a/ros_pybullet_rqt_plugin/resource/images/step_icon.png b/ros_pybullet_rqt_plugin/resource/images/step_icon.png
new file mode 100644
index 00000000..6fb2f16d
Binary files /dev/null and b/ros_pybullet_rqt_plugin/resource/images/step_icon.png differ
diff --git a/ros_pybullet_rqt_plugin/resource/images/stop_icon.png b/ros_pybullet_rqt_plugin/resource/images/stop_icon.png
new file mode 100644
index 00000000..699d62aa
Binary files /dev/null and b/ros_pybullet_rqt_plugin/resource/images/stop_icon.png differ
diff --git a/ros_pybullet_rqt_plugin/resource/ros_pybullet_rqt_plugin b/ros_pybullet_rqt_plugin/resource/ros_pybullet_rqt_plugin
new file mode 100644
index 00000000..e69de29b
diff --git a/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/__init__.py b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/__init__.py
new file mode 100644
index 00000000..e69de29b
diff --git a/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/main.py b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/main.py
new file mode 100644
index 00000000..b31354e0
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/main.py
@@ -0,0 +1,9 @@
+import sys
+from rqt_gui.main import Main
+
+def main():
+ main = Main()
+ sys.exit(main.main(sys.argv, standalone='ros_pybullet_rqt_plugin.ros2_pybullet_interface_controls.Ros2PyBulletInterfaceControls'))
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/pybullet_interface_controls_widget.py b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/pybullet_interface_controls_widget.py
new file mode 100644
index 00000000..4335cb73
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/pybullet_interface_controls_widget.py
@@ -0,0 +1,63 @@
+import os
+from ament_index_python import get_resource
+from python_qt_binding import loadUi
+from python_qt_binding.QtCore import QProcess
+from python_qt_binding.QtWidgets import QWidget
+from PyQt5.QtGui import *
+from PyQt5.QtCore import *
+from std_msgs.msg import Int64
+from std_srvs.srv import Trigger
+
+class PyBulletInterfaceControls(QWidget):
+
+ def __init__(self, node, plugin=None):
+
+ super(PyBulletInterfaceControls, self).__init__()
+
+ self._node = node
+ self.process = QProcess(self)
+ self._plugin = plugin
+
+ _, package_path = get_resource('packages', 'ros_pybullet_rqt_plugin')
+ ui_file = os.path.join(package_path, 'share', 'ros_pybullet_rqt_plugin', 'resource', 'PyBulletInterfaceControls_layout.ui')
+ loadUi(ui_file, self)
+
+ self.start_button.pressed.connect(self.start_button_handler)
+ self.send_button.pressed.connect(self.send_button_handler)
+ self.step_button.pressed.connect(self.step_button_handler)
+ self.stop_button.pressed.connect(self.stop_button_handler)
+
+ self.subscription = self._node.create_subscription(
+ Int64,
+ 'rpbi/status',
+ self.status_callback,
+ 10)
+
+
+ def start_button_handler(self):
+ self.get_srv_handler('rpbi/start', Trigger)
+
+ def step_button_handler(self):
+ self.get_srv_handler('rpbi/step', Trigger)
+
+ def stop_button_handler(self):
+ self.get_srv_handler('rpbi/stop', Trigger)
+
+ def send_button_handler(self):
+ pass
+
+ def status_callback(self, msg):
+ self.text_box.setText("Status: " + str(msg.data))
+
+ def get_srv_handler(self, srv_name: str, srv_type: type):
+ handler = None
+ handler = self._node.create_client(srv_type, srv_name)
+ handler.wait_for_service(timeout_sec = 2.0) # wait 2 seconds
+ self._node.get_logger().info('Service ' + srv_name + ' not available.')
+ return handler
+
+ def start(self):
+ pass
+
+ def shutdown_plugin(self):
+ pass
diff --git a/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/ros2_pybullet_interface_controls.py b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/ros2_pybullet_interface_controls.py
new file mode 100644
index 00000000..dbf56c31
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/ros_pybullet_rqt_plugin/ros2_pybullet_interface_controls.py
@@ -0,0 +1,20 @@
+from rqt_gui_py.plugin import Plugin
+from .pybullet_interface_controls_widget import PyBulletInterfaceControls
+
+class Ros2PyBulletInterfaceControls(Plugin):
+
+ def __init__(self, context):
+ super(Ros2PyBulletInterfaceControls, self).__init__(context)
+ self._node = context.node
+ self._logger = self._node.get_logger().get_child('ros_pybullet_rqt_plugin.ros2_pybullet_interface_controls.Ros2PyBulletInterfaceControls')
+
+ super(Ros2PyBulletInterfaceControls, self).__init__(context)
+ self.setObjectName('Ros2PyBulletInterfaceControls')
+
+ self._widget = PyBulletInterfaceControls(context.node, self)
+
+ self._widget.start()
+ if context.serial_number() > 1:
+ self._widget.setWindowTitle(
+ self._widget.windowTitle() + (' (%d)' % context.serial_number()))
+ context.add_widget(self._widget)
diff --git a/ros_pybullet_rqt_plugin/setup.cfg b/ros_pybullet_rqt_plugin/setup.cfg
new file mode 100644
index 00000000..755f45f6
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/ros_pybullet_rqt_plugin
+[install]
+install_scripts=$base/lib/ros_pybullet_rqt_plugin
diff --git a/ros_pybullet_rqt_plugin/setup.py b/ros_pybullet_rqt_plugin/setup.py
new file mode 100644
index 00000000..1fced8ac
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/setup.py
@@ -0,0 +1,32 @@
+from setuptools import setup
+from glob import glob
+
+package_name = 'ros_pybullet_rqt_plugin'
+
+setup(
+ name=package_name,
+ version='1.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ('share/' + package_name + '/resource', ['resource/PyBulletInterfaceControls_layout.ui']),
+ ('share/' + package_name, ['plugin.xml']),
+ ('share/' + package_name + '/resource/images', glob('resource/images/*.png')),
+ ('share/' + package_name + '/resource/images', glob('resource/images/*.svg')),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='Jan Kaniuka',
+ maintainer_email='jasiek491@gmail.com',
+ description='ROS2-based RQT plugin for ROS-PyBullet Interface.',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'ros_pybullet_rqt_plugin = ros_pybullet_rqt_plugin.main:main',
+ ],
+ },
+)
+
diff --git a/ros_pybullet_rqt_plugin/test/test_copyright.py b/ros_pybullet_rqt_plugin/test/test_copyright.py
new file mode 100644
index 00000000..cc8ff03f
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/test/test_copyright.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/ros_pybullet_rqt_plugin/test/test_flake8.py b/ros_pybullet_rqt_plugin/test/test_flake8.py
new file mode 100644
index 00000000..27ee1078
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/ros_pybullet_rqt_plugin/test/test_pep257.py b/ros_pybullet_rqt_plugin/test/test_pep257.py
new file mode 100644
index 00000000..b234a384
--- /dev/null
+++ b/ros_pybullet_rqt_plugin/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'