Skip to content

Commit

Permalink
consai_visualizerのテストのエラーを修正 (#640)
Browse files Browse the repository at this point in the history
  • Loading branch information
HansRobo authored Dec 14, 2024
1 parent 4d81b7c commit 171ac71
Show file tree
Hide file tree
Showing 3 changed files with 48 additions and 46 deletions.
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ repos:
rev: 24.10.0
hooks:
- id: black
args: [--line-length=99]
args: [--line-length=99, --skip-string-normalization]

- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v17.0.6
Expand Down
25 changes: 13 additions & 12 deletions consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,9 @@
# See the License for the specific language governing permissions and
# limitations under the License.

from collections import deque
import datetime
import math
from collections import deque
from typing import Dict

from consai_visualizer_msgs.msg import Color as VisColor
Expand All @@ -38,6 +38,7 @@


class FieldWidget(QWidget):

def __init__(self, parent=None):
super(FieldWidget, self).__init__(parent)

Expand Down Expand Up @@ -166,7 +167,7 @@ def paintEvent(self, event):
painter = QPainter(self)

# 背景色をセット
painter.setBrush(QColor("darkgreen"))
painter.setBrush(QColor('darkgreen'))
painter.drawRect(self.rect())

painter.save()
Expand All @@ -183,7 +184,7 @@ def paintEvent(self, event):
if self._do_rotate_draw_area is True:
painter.rotate(-90)

draw_caption = ("caption", "caption") in self._active_layers
draw_caption = ('caption', 'caption') in self._active_layers
self._draw_objects_on_transformed_area(painter, draw_caption)
self._draw_visualizer_info_on_transformed_area(painter)

Expand Down Expand Up @@ -311,8 +312,8 @@ def _draw_visualizer_info_on_transformed_area(self, painter: QPainter):
drag_line.p2.x = current_point.x()
drag_line.p2.y = current_point.y()
drag_line.size = 4
drag_line.color.name = "lightsalmon"
drag_line.caption = "dist: {:.1f} : {:.1f}, theta: {:.1f}".format(
drag_line.color.name = 'lightsalmon'
drag_line.caption = 'dist: {:.1f} : {:.1f}, theta: {:.1f}'.format(
distance.x(), distance.y(), theta_deg
)
self._draw_shape_line(painter, drag_line, True)
Expand All @@ -337,22 +338,22 @@ def _draw_visualizer_info_on_window_area(self, painter: QPainter):
average_frame_rate = sum(self._frame_rate_buffer) / self._frame_rate_buffer.maxlen
self._previous_update_time = datetime.datetime.now()
annotation = ShapeAnnotation()
annotation.text = "FPS: {:.1f}".format(average_frame_rate)
annotation.text = 'FPS: {:.1f}'.format(average_frame_rate)
annotation.normalized_x = 0.0
annotation.normalized_y = 0.95
annotation.normalized_width = 0.1
annotation.normalized_height = 0.05
annotation.color.name = "white"
annotation.color.name = 'white'
self._draw_shape_annotation(painter, annotation)

# カーソル位置を描画
cursor_pos = self._convert_draw_to_field_pos(self._mouse_current_point)
if self._invert:
annotation.text = "inv"
annotation.color.name = "lightcoral"
annotation.text = 'inv'
annotation.color.name = 'lightcoral'
else:
annotation.text = "pos"
annotation.text += " {:.2f} : {:.2f}".format(cursor_pos.x(), cursor_pos.y())
annotation.text = 'pos'
annotation.text += ' {:.2f} : {:.2f}'.format(cursor_pos.x(), cursor_pos.y())
annotation.normalized_x = 0.1
self._draw_shape_annotation(painter, annotation)

Expand All @@ -364,7 +365,7 @@ def _draw_shape_annotation(
TARGET_WIDTH = shape.normalized_width * self.width()
TARGET_HEIGHT = shape.normalized_height * self.height()

if shape.text == "":
if shape.text == '':
return

painter.save()
Expand Down
67 changes: 34 additions & 33 deletions consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,31 +15,32 @@
# limitations under the License.


from functools import partial
import math
import os
import time

# from frootspi_msgs.msg import BatteryVoltage
from functools import partial

import rclpy
from ament_index_python.resources import get_resource
from consai_visualizer_msgs.msg import ObjectsArray
from consai_visualizer.field_widget import FieldWidget
from consai_visualizer_msgs.msg import ObjectsArray
from crane_msgs.msg import RobotFeedbackArray
from python_qt_binding import loadUi
from python_qt_binding.QtCore import QPointF, Qt, QTimer
from python_qt_binding.QtWidgets import QTreeWidgetItem, QWidget
from qt_gui.plugin import Plugin
import rclpy
from robocup_ssl_msgs.msg import BallReplacement, Replacement, RobotReplacement
from rqt_py_common.ini_helper import pack, unpack
from crane_msgs.msg import RobotFeedbackArray


class Visualizer(Plugin):

def __init__(self, context):
super(Visualizer, self).__init__(context)

self.setObjectName("Visualizer")
self.setObjectName('Visualizer')

self._node = context.node
self._logger = self._node.get_logger()
Expand All @@ -48,49 +49,49 @@ def __init__(self, context):

# widgetを読み込む
# FieldWidgetはカスタムウィジェットとしてuiファイルに設定済み
pkg_name = "consai_visualizer"
_, package_path = get_resource("packages", pkg_name)
ui_file = os.path.join(package_path, "share", pkg_name, "resource", "visualizer.ui")
loadUi(ui_file, self._widget, {"FieldWidget": FieldWidget})
pkg_name = 'consai_visualizer'
_, package_path = get_resource('packages', pkg_name)
ui_file = os.path.join(package_path, 'share', pkg_name, 'resource', 'visualizer.ui')
loadUi(ui_file, self._widget, {'FieldWidget': FieldWidget})

# rqtのUIにwidgetを追加する
if context.serial_number() > 1:
self._widget.setWindowTitle(
self._widget.windowTitle() + (" (%d)" % context.serial_number())
self._widget.windowTitle() + (' (%d)' % context.serial_number())
)
context.add_widget(self._widget)

# loggerをセット
self._widget.field_widget.set_logger(self._logger)
self._add_visualizer_layer("caption", "caption")
self._add_visualizer_layer('caption', 'caption')

self._sub_visualize_objects_array = self._node.create_subscription(
ObjectsArray,
"visualizer_objects",
'visualizer_objects',
self._callback_visualizer_objects,
rclpy.qos.qos_profile_sensor_data,
)

self.sub_feedback = self._node.create_subscription(
RobotFeedbackArray,
"robot_feedback",
'robot_feedback',
self._callback_feedback,
rclpy.qos.qos_profile_sensor_data,
)

self._pub_replacement = self._node.create_publisher(Replacement, "replacement", 10)
self._pub_replacement = self._node.create_publisher(Replacement, 'replacement', 10)

# Parameterを設定する
self._widget.field_widget.set_invert(self._node.declare_parameter("invert", False).value)
self._widget.field_widget.set_invert(self._node.declare_parameter('invert', False).value)

for team in ["blue", "yellow"]:
for turnon in ["on", "off"]:
method = "self._widget.btn_all_" + turnon + "_" + team + ".clicked.connect"
for team in ['blue', 'yellow']:
for turnon in ['on', 'off']:
method = 'self._widget.btn_all_' + turnon + '_' + team + '.clicked.connect'
eval(method)(
partial(
self._publish_all_robot_turnon_replacement,
team == "yellow",
turnon == "on",
team == 'yellow',
turnon == 'on',
)
)

Expand Down Expand Up @@ -126,15 +127,15 @@ def save_settings(self, plugin_settings, instance_settings):

# layerとsub layerをカンマで結合して保存
active_layers = self._extract_active_layers()
combined_layers = list(map(lambda x: x[0] + "," + x[1], active_layers))
instance_settings.set_value("active_layers", pack(combined_layers))
combined_layers = [x[0] + ',' + x[1] for x in active_layers]
instance_settings.set_value('active_layers', pack(combined_layers))

def restore_settings(self, plugin_settings, instance_settings):
# UIが起動したときに実行される関数

# カンマ結合されたlayerを復元してセット
combined_layers = unpack(instance_settings.value("active_layers", []))
active_layers = list(map(lambda x: x.split(","), combined_layers))
combined_layers = unpack(instance_settings.value('active_layers', []))
active_layers = [x.split(',') for x in combined_layers]
for layer, sub_layer in active_layers:
self._add_visualizer_layer(layer, sub_layer, Qt.Checked)

Expand All @@ -152,8 +153,8 @@ def _callback_visualizer_objects(self, msg):

def _add_visualizer_layer(self, layer: str, sub_layer: str, state=Qt.Unchecked):
# レイヤーに重複しないように項目を追加する
if layer == "" or sub_layer == "":
self._logger.warning("layer={} or sub_layer={} is empty".format(layer, sub_layer))
if layer == '' or sub_layer == '':
self._logger.warning('layer={} or sub_layer={} is empty'.format(layer, sub_layer))
return

parents = self._widget.layer_widget.findItems(layer, Qt.MatchExactly, 0)
Expand Down Expand Up @@ -199,14 +200,14 @@ def _publish_replacement(self) -> None:

# チェックが入ったボタン情報を解析する
button = self._widget.radio_buttons.checkedButton()
if button.text() == "NONE":
if button.text() == 'NONE':
return
elif button.text() == "Ball":
elif button.text() == 'Ball':
self._publish_ball_replacement(start, end)
return
else:
is_yellow = False
if button.text()[0] == "Y":
if button.text()[0] == 'Y':
is_yellow = True
robot_id = int(button.text()[1:])
self._publish_robot_replacement(start, end, is_yellow, robot_id)
Expand Down Expand Up @@ -265,26 +266,26 @@ def _update_robot_synthetics(self):
diff_time = now - self.latest_update_time[i]

try:
getattr(self._widget, f"robot{i}_voltage").setText(
getattr(self._widget, f'robot{i}_voltage').setText(
str(self.latest_battery_voltage[i])
)
except AttributeError:
try:
getattr(self._widget, f"robot{i}_voltage").setText(str(0.0))
getattr(self._widget, f'robot{i}_voltage').setText(str(0.0))
except AttributeError:
pass
pass
if diff_time > 3.0: # 死んだ判定
# DEATH
try:
getattr(self._widget, f"robot{i}_connection_status").setText("❌")
getattr(self._widget, f'robot{i}_connection_status').setText('❌')
except AttributeError:
# ロボット状態表示UIは12列しか用意されておらず、ID=12以降が来るとエラーになるため回避
pass
else:
# ALIVE
try:
getattr(self._widget, f"robot{i}_connection_status").setText("👍")
getattr(self._widget, f'robot{i}_connection_status').setText('👍')
except AttributeError:
# ロボット状態表示UIは12列しか用意されておらず、ID=12以降が来るとエラーになるため回避
pass

0 comments on commit 171ac71

Please sign in to comment.