diff --git a/README.md b/README.md index cedba6e..cd823cf 100644 --- a/README.md +++ b/README.md @@ -99,11 +99,11 @@ GitHubのProjects機能で開発方針に合わせたタスク管理をしてい - [2nd step - SSL Game Example](https://github.com/SSL-Roots/consai2r2/projects/3) - [3rd step - use ROS2 effectively](https://github.com/SSL-Roots/consai2r2/projects/2) -# Controbution +# Contribution issue, pull request 大歓迎です。 -[CONTORIBUTING.md](./CONTRIBUTING.md)を見てくれると嬉しいです。 +[CONTRIBUTING.md](./CONTRIBUTING.md)を見てくれると嬉しいです。 # Author & Contributors diff --git a/consai2r2_description/CMakeLists.txt b/consai2r2_description/CMakeLists.txt index 36a78e1..0e51968 100644 --- a/consai2r2_description/CMakeLists.txt +++ b/consai2r2_description/CMakeLists.txt @@ -13,7 +13,6 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(python_cmake_module REQUIRED) set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}") @@ -30,7 +29,6 @@ add_executable(${PROJECT_NAME}_node ament_target_dependencies( ${PROJECT_NAME}_node "rclcpp" - "launch_ros" ) diff --git a/consai2r2_description/consai2r2_description/__init__.py b/consai2r2_description/consai2r2_description/__init__.py index 5fdfdf3..4c15d64 100755 --- a/consai2r2_description/consai2r2_description/__init__.py +++ b/consai2r2_description/consai2r2_description/__init__.py @@ -23,8 +23,8 @@ class consai2r2_parameters(): - def __init__(self, node): - param_names = consai2r2_description.parameter.list_parameters(node) - params = consai2r2_description.parameter.get_parameters(node, param_names) + def __init__(self, node, timeout_sec=10.0): + param_names = consai2r2_description.parameter.list_parameters(node, timeout_sec) + params = consai2r2_description.parameter.get_parameters(node, param_names, timeout_sec) for param_name, param_value in params.items(): setattr(self, param_name, param_value) diff --git a/consai2r2_description/consai2r2_description/parameter.py b/consai2r2_description/consai2r2_description/parameter.py index fef2bfc..4149d73 100755 --- a/consai2r2_description/consai2r2_description/parameter.py +++ b/consai2r2_description/consai2r2_description/parameter.py @@ -19,15 +19,15 @@ import rclpy -# https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py#L174 -def list_parameters(node): +# https://github.com/ros2/ros2cli/blob/780923c046f8e537e884d18bef33a2338f2d409c/ros2param/ros2param/api/__init__.py#L174 +def list_parameters(node, timeout_sec=10.0): # create client client = node.create_client( ListParameters, 'consai2r2_description/list_parameters') # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) + ready = client.wait_for_service(timeout_sec) if not ready: raise RuntimeError('Wait for service timed out') @@ -43,15 +43,15 @@ def list_parameters(node): return response.result.names -# https://github.com/ros2/ros2cli/blob/master/ros2param/ros2param/api/__init__.py#L54 -def get_parameters(node, parameter_names): +# https://github.com/ros2/ros2cli/blob/780923c046f8e537e884d18bef33a2338f2d409c/ros2param/ros2param/api/__init__.py#L122 +def get_parameters(node, parameter_names, timeout_sec=10.0): # create client client = node.create_client( GetParameters, 'consai2r2_description/get_parameters') # call as soon as ready - ready = client.wait_for_service(timeout_sec=5.0) + ready = client.wait_for_service(timeout_sec) if not ready: raise RuntimeError('Wait for service timed out') @@ -68,6 +68,7 @@ def get_parameters(node, parameter_names): return_values = {} + # https://github.com/ros2/ros2cli/blob/780923c046f8e537e884d18bef33a2338f2d409c/ros2param/ros2param/api/__init__.py#L54 for i, pvalue in enumerate(response.values): if pvalue.type == ParameterType.PARAMETER_BOOL: value = pvalue.bool_value diff --git a/consai2r2_examples/launch/joystick_example.launch.py b/consai2r2_examples/launch/joystick_example.launch.py index e033011..396bdd1 100644 --- a/consai2r2_examples/launch/joystick_example.launch.py +++ b/consai2r2_examples/launch/joystick_example.launch.py @@ -18,6 +18,8 @@ # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN # THE SOFTWARE. +import os +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration @@ -49,7 +51,9 @@ def generate_launch_description(): start_sender_cmd = Node( package='consai2r2_sender', node_executable='sim_sender', - output='screen' + output='screen', + parameters=[os.path.join(get_package_share_directory( + 'consai2r2_sender'), 'config', 'grsim.yaml')] ) ld = LaunchDescription() diff --git a/consai2r2_examples/package.xml b/consai2r2_examples/package.xml index 2f79e57..5af456e 100644 --- a/consai2r2_examples/package.xml +++ b/consai2r2_examples/package.xml @@ -9,6 +9,7 @@ ament_cmake launch_ros + ament_index_python joy ament_lint_auto ament_lint_common diff --git a/consai2r2_gameviewer/CMakeLists.txt b/consai2r2_gameviewer/CMakeLists.txt new file mode 100644 index 0000000..a0d30db --- /dev/null +++ b/consai2r2_gameviewer/CMakeLists.txt @@ -0,0 +1,23 @@ +cmake_minimum_required(VERSION 3.5) +project(consai2r2_gameviewer) + +# Load ament and all dependencies required for this package +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_python_install_package(${PROJECT_NAME} + PACKAGE_DIR src/${PROJECT_NAME}) + +install(FILES plugin.xml + DESTINATION share/${PROJECT_NAME} +) + +install(DIRECTORY resource + DESTINATION share/${PROJECT_NAME} +) + +install(PROGRAMS scripts/consai2r2_gameviewer + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/consai2r2_gameviewer/package.xml b/consai2r2_gameviewer/package.xml new file mode 100644 index 0000000..3da52a4 --- /dev/null +++ b/consai2r2_gameviewer/package.xml @@ -0,0 +1,24 @@ + + + + consai2r2_gameviewer + 0.0.0 + consai2r2_gameviewer provides a GUI plugin for displaying vision and referee messages. + akshota + MIT + + ament_cmake + + ament_index_python + python_qt_binding + rclpy + rqt_gui + rqt_gui_py + rqt_py_common + + + + + ament_cmake + + diff --git a/consai2r2_gameviewer/plugin.xml b/consai2r2_gameviewer/plugin.xml new file mode 100644 index 0000000..8217d8d --- /dev/null +++ b/consai2r2_gameviewer/plugin.xml @@ -0,0 +1,17 @@ + + + + A Python GUI plugin for displaying vision and referee messages. + + + + + folder + Plugins related to visualization. + + + image-x-generic + A Python GUI plugin for displaying vision and referee messages. + + + diff --git a/consai2r2_gameviewer/resource/gameviewer_widget.ui b/consai2r2_gameviewer/resource/gameviewer_widget.ui new file mode 100644 index 0000000..1f346fa --- /dev/null +++ b/consai2r2_gameviewer/resource/gameviewer_widget.ui @@ -0,0 +1,19 @@ + + + GameViewerWidget + + + + 0 + 0 + 400 + 300 + + + + GameViewer + + + + + diff --git a/consai2r2_gameviewer/scripts/consai2r2_gameviewer b/consai2r2_gameviewer/scripts/consai2r2_gameviewer new file mode 100644 index 0000000..bcb9058 --- /dev/null +++ b/consai2r2_gameviewer/scripts/consai2r2_gameviewer @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import sys + +from rqt_gui.main import Main + +main = Main() +sys.exit(main.main(sys.argv, standalone='consai2r2_gameviewer.gameviewer.GameViewer')) diff --git a/consai2r2_gameviewer/src/consai2r2_gameviewer/__init__.py b/consai2r2_gameviewer/src/consai2r2_gameviewer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer.py b/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer.py new file mode 100644 index 0000000..62826ec --- /dev/null +++ b/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer.py @@ -0,0 +1,62 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +from qt_gui.plugin import Plugin +from python_qt_binding.QtCore import QTimer + +from consai2r2_gameviewer.gameviewer_widget import GameViewerWidget + + +class GameViewer(Plugin): + + """ + consai2r2_gameviwer plugin's main class. + """ + + def __init__(self, context): + """ + :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, + ''PluginContext'' + """ + super(GameViewer, self).__init__(context) + self.setObjectName('GameViewer') + + self._context = context + + self._widget = GameViewerWidget() + if context.serial_number() > 1: + self._widget.setWindowTitle( + self._widget.windowTitle() + (' (%d)' % context.serial_number())) + context.add_widget(self._widget) + + self._timer = QTimer() + self._timer.timeout.connect(self._widget.update) + self._timer.start(16) # the argument is msec interval + + def shutdown_plugin(self): + self._timer.stop() + pass + + def save_settings(self, plugin_settings, instance_settings): + pass + + def restore_settings(self, plugin_settings, instance_settings): + pass + diff --git a/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer_widget.py b/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer_widget.py new file mode 100644 index 0000000..78ede74 --- /dev/null +++ b/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer_widget.py @@ -0,0 +1,57 @@ +# Copyright (c) 2019 SSL-Roots +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. + +import os + +from ament_index_python.resources import get_resource + +from python_qt_binding import loadUi +from python_qt_binding.QtWidgets import QWidget +from python_qt_binding.QtGui import QPainter +from python_qt_binding.QtCore import Qt + +class GameViewerWidget(QWidget): + + """ + Primary widget for the consai2r2_gameviewer plugin. + """ + + def __init__(self): + super(GameViewerWidget, self).__init__() + + pkg_name = 'consai2r2_gameviewer' + _, package_path = get_resource('packages', pkg_name) + ui_file = os.path.join( + package_path, 'share', pkg_name, 'resource', 'gameviewer_widget.ui') + loadUi(ui_file, self) + + self.setObjectName('GameViewerWidget') + + + def paintEvent(self, event): + painter = QPainter(self) + + # Hello world + painter.setBrush(Qt.green) + painter.setPen(Qt.black) + painter.drawRect(self.rect()) + + painter.drawText(self.rect().width()*0.5, self.rect().height()*0.5, "Hello world!") + diff --git a/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp b/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp index 475b182..b62fb64 100644 --- a/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp +++ b/consai2r2_receiver/include/consai2r2_receiver/multicast.hpp @@ -34,22 +34,23 @@ class MulticastReceiver { public: MulticastReceiver(const std::string & ip, const int port) - : endpoint(asio::ip::udp::v4(), port), socket(io_service, endpoint) + : socket_(io_service_, asio::ip::udp::v4()) { asio::ip::address addr = asio::ip::address::from_string(ip); if (!addr.is_multicast()) { - throw std::runtime_error("excpeted multicast address"); + throw std::runtime_error("expected multicast address"); } - socket.set_option(asio::ip::multicast::join_group(addr.to_v4())); - socket.set_option(asio::socket_base::reuse_address(true)); - socket.non_blocking(true); + socket_.set_option(asio::socket_base::reuse_address(true)); + socket_.set_option(asio::ip::multicast::join_group(addr.to_v4())); + socket_.bind(asio::ip::udp::endpoint(asio::ip::udp::v4(), port)); + socket_.non_blocking(true); } size_t receive(std::vector & msg) { boost::system::error_code error; - const size_t received = socket.receive_from(asio::buffer(msg), endpoint, 0, error); + const size_t received = socket_.receive(asio::buffer(msg), 0, error); if (error && error != asio::error::message_size) { throw boost::system::system_error(error); return 0; @@ -57,12 +58,11 @@ class MulticastReceiver return received; } - size_t available() {return socket.available();} + size_t available() {return socket_.available();} private: - asio::io_service io_service; - asio::ip::udp::endpoint endpoint; - asio::ip::udp::socket socket; + asio::io_service io_service_; + asio::ip::udp::socket socket_; }; #endif // CONSAI2R2_RECEIVER__MULTICAST_HPP_ diff --git a/consai2r2_sender/CMakeLists.txt b/consai2r2_sender/CMakeLists.txt index b211072..25813de 100644 --- a/consai2r2_sender/CMakeLists.txt +++ b/consai2r2_sender/CMakeLists.txt @@ -49,6 +49,16 @@ install(TARGETS sim_sender EXPORT export_${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/consai2r2_sender/README.md b/consai2r2_sender/README.md new file mode 100644 index 0000000..d83eabd --- /dev/null +++ b/consai2r2_sender/README.md @@ -0,0 +1,53 @@ +# consai2r2_sender + +ロボット(grSim)へ動作司令を送信するパッケージです。 + + + +## ノードの起動方法 + +次のコマンドでsenderが起動します。 + +```sh +ros2 launch consai2r2_sender sender.launch.py +``` + + + + + + + +## 参考ページ +### Google proto files +- grSim + - https://github.com/RoboCup-SSL/grSim/tree/master/src/proto diff --git a/consai2r2_sender/config/grsim.yaml b/consai2r2_sender/config/grsim.yaml new file mode 100644 index 0000000..0c57296 --- /dev/null +++ b/consai2r2_sender/config/grsim.yaml @@ -0,0 +1,4 @@ +consai2r2_sim_sender: + ros__parameters: + grsim_addr: '127.0.0.1' + grsim_port: 20011 diff --git a/consai2r2_description/launch/description.launch.py b/consai2r2_sender/launch/sender.launch.py similarity index 58% rename from consai2r2_description/launch/description.launch.py rename to consai2r2_sender/launch/sender.launch.py index 0e35c31..2b86d28 100644 --- a/consai2r2_description/launch/description.launch.py +++ b/consai2r2_sender/launch/sender.launch.py @@ -19,30 +19,21 @@ # THE SOFTWARE. import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): - config_path = os.path.join( - get_package_share_directory('consai2r2_description'), 'config', 'config.yaml') + start_sender_cmd = Node( + package='consai2r2_sender', node_executable='sim_sender', + output='screen', + parameters=[os.path.join(get_package_share_directory( + 'consai2r2_sender'), 'config', 'grsim.yaml')] + ) + + ld = LaunchDescription() + + ld.add_action(start_sender_cmd) - return LaunchDescription([ - Node( - package='consai2r2_description', node_executable='consai2r2_description_node', - output='screen', parameters=[config_path]), - Node( - package='consai2r2_receiver', node_executable='referee_receiver', - output='screen', parameters=[config_path]), - Node( - package='consai2r2_receiver', node_executable='vision_receiver', - output='screen', parameters=[config_path]), - Node( - package='consai2r2_vision_wrapper', node_executable='vision_wrapper', - output='screen', parameters=[config_path]), - Node( - package='consai2r2_referee_wrapper', node_executable='referee_wrapper', - output='screen', parameters=[config_path]), - ]) + return ld diff --git a/consai2r2_sender/package.xml b/consai2r2_sender/package.xml index bf2df8d..9ebd0d8 100644 --- a/consai2r2_sender/package.xml +++ b/consai2r2_sender/package.xml @@ -15,6 +15,8 @@ protobuf-dev boost + ament_index_python + launch_ros ament_lint_auto ament_lint_common diff --git a/consai2r2_sender/src/sim_sender.cpp b/consai2r2_sender/src/sim_sender.cpp index 2590b09..3145d96 100644 --- a/consai2r2_sender/src/sim_sender.cpp +++ b/consai2r2_sender/src/sim_sender.cpp @@ -63,14 +63,10 @@ class SimSender : public rclcpp::Node SimSender() : Node("consai2r2_sim_sender") { - std::string host; - int port; - this->declare_parameter("grsim_addr", "127.0.0.1"); this->declare_parameter("grsim_port", 20011); - - this->get_parameter("grsim_addr", host); - this->get_parameter("grsim_port", port); + auto host = this->get_parameter("grsim_addr").as_string(); + auto port = this->get_parameter("grsim_port").as_int(); sub_commands_ = this->create_subscription( "robot_commands", 10, std::bind(&SimSender::send_commands, this, std::placeholders::_1));