Merge branch 'master' into dev/vision-wrapper

spiralray · spiralray · commit 93ba48cbbd8d · 2020-01-07T23:53:10.000+09:00
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -9,6 +9,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <exec_depend>launch_ros</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
   <exec_depend>joy</exec_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/consai2r2_gameviewer/CMakeLists.txt 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
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>consai2r2_gameviewer</name>
+  <version>0.0.0</version>
+  <description>consai2r2_gameviewer provides a GUI plugin for displaying vision and referee messages.</description>
+  <maintainer email="macakasit@gmail.com">akshota</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend version_gte="0.2.19">python_qt_binding</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rqt_gui</exec_depend>
+  <exec_depend>rqt_gui_py</exec_depend>
+  <exec_depend>rqt_py_common</exec_depend>
+
+  <export>
+    <!-- <architecture_independent/> -->
+    <rqt_gui plugin="${prefix}/plugin.xml"/>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/consai2r2_gameviewer/plugin.xml b/consai2r2_gameviewer/plugin.xml
@@ -0,0 +1,17 @@
+<library path="src">
+  <class name="GameViewer" type="consai2r2_gameviewer.gameviewer.GameViewer" base_class_type="rqt_gui_py::Plugin">
+    <description>
+      A Python GUI plugin for displaying vision and referee messages.
+    </description>
+    <qtgui>
+      <group>
+        <label>Visualization</label>
+        <icon type="theme">folder</icon>
+        <statustip>Plugins related to visualization.</statustip>
+      </group>
+      <label>CON-SAI2 R2 GameViewer</label>
+      <icon type="theme">image-x-generic</icon>
+      <statustip>A Python GUI plugin for displaying vision and referee messages.</statustip>
+    </qtgui>
+  </class>
+</library>
diff --git a/consai2r2_gameviewer/resource/gameviewer_widget.ui b/consai2r2_gameviewer/resource/gameviewer_widget.ui
@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>GameViewerWidget</class>
+ <widget class="QWidget" name="GameViewerWidget">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>400</width>
+    <height>300</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>GameViewer</string>
+  </property>
+ </widget>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/consai2r2_gameviewer/scripts/consai2r2_gameviewer 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
diff --git a/consai2r2_gameviewer/src/consai2r2_gameviewer/gameviewer.py 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
@@ -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
@@ -34,35 +34,35 @@ 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<char> & 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;
     }
     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
@@ -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
diff --git a/consai2r2_sender/config/grsim.yaml b/consai2r2_sender/config/grsim.yaml
diff --git a/consai2r2_sender/launch/sender.launch.py b/consai2r2_sender/launch/sender.launch.py
diff --git a/consai2r2_sender/package.xml b/consai2r2_sender/package.xml
diff --git a/consai2r2_sender/src/sim_sender.cpp b/consai2r2_sender/src/sim_sender.cpp
