diff --git a/consai_ros2/consai_visualizer/resource/visualizer.ui b/consai_ros2/consai_visualizer/resource/visualizer.ui index a02a3d780..3f50a49ef 100644 --- a/consai_ros2/consai_visualizer/resource/visualizer.ui +++ b/consai_ros2/consai_visualizer/resource/visualizer.ui @@ -39,7 +39,7 @@ - Connection + ping Qt::AlignCenter @@ -49,7 +49,7 @@ - Battery + 電圧 Qt::AlignCenter @@ -59,7 +59,7 @@ - Error + リターン Qt::AlignCenter @@ -95,24 +95,24 @@ - ConnectionStatus + - Qt::AlignCenter - + - TextLabel + - - - + + - TextLabel + - @@ -145,21 +145,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -183,7 +183,7 @@ - + @@ -192,21 +192,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -227,7 +227,7 @@ - + @@ -236,21 +236,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -280,21 +280,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -324,21 +324,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -368,21 +368,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -412,21 +412,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -456,21 +456,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -500,21 +500,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -544,21 +544,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - @@ -588,21 +588,21 @@ - ConnectionStatus + - - + - TextLabel + - - - + + - TextLabel + - diff --git a/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py b/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py index 844c47798..bab03681e 100644 --- a/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py +++ b/consai_ros2/consai_visualizer/src/consai_visualizer/visualizer.py @@ -28,7 +28,7 @@ from crane_visualization_interfaces.msg import ObjectsArray from python_qt_binding import loadUi from python_qt_binding.QtCore import QPointF, Qt, QTimer -from python_qt_binding.QtWidgets import QTreeWidgetItem, QWidget +from python_qt_binding.QtWidgets import QTreeWidgetItem, QWidget, QFrame from qt_gui.plugin import Plugin import rclpy from robocup_ssl_msgs.msg import BallReplacement, Replacement, RobotReplacement @@ -125,7 +125,7 @@ def __init__(self, context): self._reset_timer.timeout.connect(self._update_robot_synthetics) self._reset_timer.start(1000) - self.latest_battery_voltage = [0] * 16 + self.latest_battery_voltage = [-1] * 16 # self._widget.pushButton.clicked.connect(self.publish) self._widget.session_injection_comboBox.addItem("simple_ai") @@ -179,6 +179,8 @@ def _session_injection(self): self._pub_session_injection.publish(msg) def _callback_feedback(self, msg): + self.latest_return_counter = [-1] * 16 + self.latest_battery_voltage = [-1] * 16 for feedback in msg.feedback: try: self.latest_battery_voltage[feedback.robot_id] = feedback.voltage[0] @@ -186,6 +188,10 @@ def _callback_feedback(self, msg): except AttributeError: # 初期化より先にコールバックが呼ばれてしまうことがあるため、エラーを回避する pass + try: + self.latest_return_counter[feedback.robot_id] = feedback.counter + except AttributeError: + pass # for synthetics def _callback_ping(self, msg): @@ -335,12 +341,19 @@ def _update_robot_synthetics(self): for i in range(16): # 電圧 try: - getattr(self._widget, f"robot{i}_voltage").setText( - "{:.2f}".format(self.latest_battery_voltage[i]) - ) + label = getattr(self._widget, f"robot{i}_voltage") + if self.latest_battery_voltage[i] == -1: + label.setText("-") + label.setLineWidth(0) + else: + label.setText("{:.2f}".format(self.latest_battery_voltage[i])) + label.setLineWidth(1) + label.setFrameStyle(QFrame.Box | QFrame.Plain) except AttributeError: try: - getattr(self._widget, f"robot{i}_voltage").setText(str(0.0)) + label = getattr(self._widget, f"robot{i}_voltage") + label.setText("-") + label.setLineWidth(0) except AttributeError: pass pass @@ -349,14 +362,34 @@ def _update_robot_synthetics(self): try: # 一旦全て"-"で埋める for i in range(12): - getattr(self._widget, f"robot{i}_connection_status").setText("-") + label = getattr(self._widget, f"robot{i}_connection_status") + label.setText("-") + label.setLineWidth(0) for ping_status in self.ping.ping: - getattr( + label = getattr( self._widget, f"robot{ping_status.robot_id}_connection_status" - ).setText("{:.1f}ms".format(ping_status.ping_ms)) + ) + label.setText("{:.1f}ms".format(ping_status.ping_ms)) + label.setLineWidth(1) + label.setFrameStyle(QFrame.Box | QFrame.Plain) except AttributeError: try: - getattr(self._widget, f"robot{i}_connection_status").setText("-") + label = getattr(self._widget, f"robot{i}_connection_status") + label.setText("-") + label.setLineWidth(0) except AttributeError: pass pass + + try: + for i in range(12): + label = getattr(self._widget, f"robot{i}_counter") + if self.latest_return_counter[i] == -1: + label.setText("-") + label.setLineWidth(0) + else: + label.setText(str(self.latest_return_counter[i])) + label.setLineWidth(1) + label.setFrameStyle(QFrame.Box | QFrame.Plain) + except AttributeError: + pass diff --git a/crane_msgs/msg/control/RobotCommand.msg b/crane_msgs/msg/control/RobotCommand.msg index 0e3fcdab7..bd1cef3fc 100755 --- a/crane_msgs/msg/control/RobotCommand.msg +++ b/crane_msgs/msg/control/RobotCommand.msg @@ -29,7 +29,7 @@ geometry_msgs/Pose2D current_pose bool enable_local_feedback float32 target_theta -float32 omega_limit 100.0 +float32 omega_limit 10.0 float32 latency_ms 0.0 diff --git a/crane_robot_receiver/src/robot_receiver_node.cpp b/crane_robot_receiver/src/robot_receiver_node.cpp index f97594fff..7dccdb70c 100644 --- a/crane_robot_receiver/src/robot_receiver_node.cpp +++ b/crane_robot_receiver/src/robot_receiver_node.cpp @@ -28,7 +28,7 @@ struct RobotInterfaceConfig auto makeConfig(uint8_t id) -> RobotInterfaceConfig { RobotInterfaceConfig config; - config.ip = "224.5.20.100"; + config.ip = std::format("224.5.20.{}", id + 100); config.port = 50100 + id; return config; } diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp index 187406a98..f3a85d753 100644 --- a/crane_sender/src/ibis_sender_node.cpp +++ b/crane_sender/src/ibis_sender_node.cpp @@ -158,7 +158,7 @@ class IbisSenderNode : public SenderBase packet.stop_emergency = command.stop_flag; packet.acceleration_limit = command.local_planner_config.max_acceleration + 1.0; packet.linear_velocity_limit = command.local_planner_config.max_velocity; - packet.angular_velocity_limit = 10.; + packet.angular_velocity_limit = command.omega_limit; packet.prioritize_move = true; packet.prioritize_accurate_acceleration = true; diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp index 83167a997..75242ef47 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp @@ -132,10 +132,14 @@ class WorldModelPublisherComponent : public rclcpp::Node double ball_placement_target_y; - bool ball_detected[20] = {}; + bool ball_sensor_detected[20] = {}; crane_msgs::msg::BallInfo ball_info; + std::deque ball_info_history; + + rclcpp::Time last_ball_detect_time; + std::vector robot_info[2]; std::unique_ptr geometry_receiver; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 324dadcbd..92061b146 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -68,7 +68,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (feedback->ball_sensor) { contact.last_contacted_time = now; } - ball_detected[robot.robot_id] = feedback->ball_sensor; + ball_sensor_detected[robot.robot_id] = feedback->ball_sensor; } } }); @@ -78,7 +78,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (our_color == Color::BLUE) { auto now = rclcpp::Clock().now(); for (auto status : msg->robots_status) { - ball_detected[status.robot_id] = status.infrared; + ball_sensor_detected[status.robot_id] = status.infrared; auto & contact = robot_info[static_cast(our_color)][status.robot_id].ball_contact; contact.current_time = now; @@ -95,7 +95,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (our_color == Color::YELLOW) { auto now = rclcpp::Clock().now(); for (auto status : msg->robots_status) { - ball_detected[status.robot_id] = status.infrared; + ball_sensor_detected[status.robot_id] = status.infrared; auto & contact = robot_info[static_cast(our_color)][status.robot_id].ball_contact; contact.current_time = now; @@ -205,6 +205,12 @@ void WorldModelPublisherComponent::on_udp_timer() if (packet.has_geometry()) { visionGeometryCallback(packet.geometry()); } + if (packet.has_detection()) { + int balls_size = packet.detection().balls().size(); + if (0 > balls_size) { + last_ball_detect_time = now(); + } + } } } } @@ -264,6 +270,24 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame & ball_info.disappeared = false; } else { ball_info.detected = false; + + // ball disappeared 判定 + int ball_info_history_size = ball_info_history.size(); + if (1 > ball_info_history_size) { + auto last_ball_info_history = ball_info_history[ball_info_history_size - 1]; + // double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time; + double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds(); + + // 0.5secビジョンから見えていなければ見失った + if (0.5 < elapsed_time_since_last_detected) { + ball_info.disappeared = true; + } + } + } + + ball_info_history.emplace_back(ball_info); + if (ball_info_history.size() > 10) { + ball_info_history.pop_front(); } has_vision_updated = true; @@ -303,11 +327,12 @@ void WorldModelPublisherComponent::publishWorldModel() wm.is_yellow = (our_color == Color::YELLOW); wm.on_positive_half = on_positive_half; wm.is_emplace_positive_side = is_emplace_positive_side; - wm.ball_info = ball_info; wm.our_max_allowed_bots = our_max_allowed_bots; wm.their_max_allowed_bots = their_max_allowed_bots; updateBallContact(); + wm.ball_info = ball_info; + ball_history.push_back(Point(wm.ball_info.pose.x, wm.ball_info.pose.y)); if (ball_history.size() > history_size) { ball_history.pop_front(); @@ -455,12 +480,6 @@ void WorldModelPublisherComponent::publishWorldModel() void WorldModelPublisherComponent::updateBallContact() { auto now = rclcpp::Clock().now(); - static std::deque ball_info_history; - - ball_info_history.emplace_back(ball_info); - if (ball_info_history.size() > 10) { - ball_info_history.pop_front(); - } // bool pre_is_our_ball = std::exchange(is_our_ball, false); is_their_ball = false; @@ -524,12 +543,32 @@ void WorldModelPublisherComponent::updateBallContact() // ローカルセンサーの情報でボール情報を更新 for (std::size_t i = 0; i < robot_info[static_cast(our_color)].size(); i++) { - // ボールがロボットに近いときのみ接触とみなす（誤作動防止） double ball_distance = std::hypot( ball_info.pose.x - robot_info[static_cast(our_color)][i].pose.x, ball_info.pose.y - robot_info[static_cast(our_color)][i].pose.y); + // ビジョンがボールを見失っているときにボールセンサが反応している間は、接触しているものとみなす。 if ( - ball_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && + ball_sensor_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && + ball_info.disappeared) { + // ビジョンはボール見失っているけどロボットが保持しているので、ロボットの座標にボールがあることにする + + Point center_to_kicker = + getNormVec(robot_info[static_cast(our_color)][i].pose.theta) * 0.09; + ball_info.pose.x = + robot_info[static_cast(our_color)][i].pose.x + center_to_kicker.x(); + ball_info.pose.y = + robot_info[static_cast(our_color)][i].pose.y + center_to_kicker.y(); + + robot_info[static_cast(our_color)][i].ball_contact.is_vision_source = false; + robot_info[static_cast(our_color)][i].ball_contact.current_time = now; + robot_info[static_cast(our_color)][i].ball_contact.last_contacted_time = now; + if (not is_our_ball) { + std::cout << "敵ボール接触" << std::endl; + is_our_ball = true; + ball_event_detected = true; + } + } else if ( + ball_sensor_detected[i] && not robot_info[static_cast(our_color)][i].disappeared && ball_distance < 0.3) { robot_info[static_cast(our_color)][i].ball_contact.is_vision_source = false; robot_info[static_cast(our_color)][i].ball_contact.current_time = now;