ROS 2 Debug Draw Interface (#1582)

* Add shape messages and converters

* Add autonomy messages and associated converters

* Remove deleted items (i.e. sing) from Robot

* Fix conversions and usage to reflect RobotIntent changes

* Debug drawer

* Formatting

* Formatting

* Formatting

* Fix include

* Fix RTP conversions with new intent format

* Link to rj_drawing_msgs

* Make song optional in log frame

* Fix test

* Fix tests 2 electric boogaloo

* Tidy

* Formatting and nolint

* Fix TODO

* Pretty fixes and TODO #s

* Trailing newline

* Fix review comments, cleanup

CMakeLists.txt

@@ -94,6 +94,7 @@ include(AddROSIDLSubdirectory)
 # First call add_idl_subdirectory on all ROSIDL subdirectories. This will build + install them at
 # configure time so that find_package will work on them.
 add_rosidl_subdirectory(rj_geometry_msgs)
+add_rosidl_subdirectory(rj_drawing_msgs)
 add_rosidl_subdirectory(rj_msgs)
 
 # This is manually topologically sorted because we're not using colcon

rj_constants/include/rj_constants/topic_names.hpp

@@ -14,6 +14,12 @@ constexpr auto kGameSettingsSrv = "config/set_game_settings";
 constexpr auto kFieldDimensionsSrv = "config/set_field_dimensions";
 }  // namespace config_server::topics
 
+namespace viz::topics {
+
+constexpr auto kDebugDrawPub = "viz/debug_draw";
+
+}  // namespace viz::topics
+
 namespace referee::topics {
 constexpr auto kGameStatePub = "referee/game_state";
 constexpr auto kOurInfoPub = "referee/our_info";

rj_drawing_msgs/CMakeLists.txt

@@ -0,0 +1,40 @@
+# ======================================================================
+# Preamble
+# ======================================================================
+cmake_minimum_required(VERSION 3.16)
+project(rj_drawing_msgs)
+
+# ======================================================================
+# Find package
+# ======================================================================
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rj_geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
+
+# ======================================================================
+# Define Targets
+# ======================================================================
+# This creates the target rj_drawing_msgs__rosidl_typesupport_cpp
+rosidl_generate_interfaces(
+  rj_drawing_msgs
+  # Messages
+  msg/DebugDraw.msg
+  msg/DrawColor.msg
+  msg/DrawShapes.msg
+  msg/DrawSegment.msg
+  msg/DrawPose.msg
+  msg/DrawPath.msg
+  msg/DrawText.msg
+
+  DEPENDENCIES
+  std_msgs
+  builtin_interfaces
+  rj_geometry_msgs)
+
+# ======================================================================
+# ROS2 packaging
+# ======================================================================
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

rj_drawing_msgs/msg/DebugDraw.msg

@@ -0,0 +1,9 @@
+builtin_interfaces/Time stmp
+
+string layer
+
+DrawShapes[] shapes
+DrawSegment[] segments
+DrawPose[] poses
+DrawPath[] paths
+DrawText[] debug_text

rj_drawing_msgs/msg/DrawColor.msg

@@ -0,0 +1,4 @@
+int8 r
+int8 g
+int8 b
+int8 a

rj_drawing_msgs/msg/DrawPath.msg

@@ -0,0 +1,5 @@
+# Represents a 2-dimensional path (with no velocity/time component).
+rj_geometry_msgs/Point[] points
+
+# If this has one element, the whole path is the same color. Otherwise, each point gets its own color.
+DrawColor[] colors

rj_drawing_msgs/msg/DrawPose.msg

@@ -0,0 +1,2 @@
+rj_geometry_msgs/Pose pose
+DrawColor color

rj_drawing_msgs/msg/DrawSegment.msg

@@ -0,0 +1,2 @@
+rj_geometry_msgs/Segment segment
+DrawColor color

rj_drawing_msgs/msg/DrawShapes.msg

@@ -0,0 +1,2 @@
+rj_geometry_msgs/ShapeSet shapes
+DrawColor color

rj_drawing_msgs/msg/DrawText.msg

@@ -0,0 +1,3 @@
+string text
+rj_geometry_msgs/Point position
+DrawColor color

rj_drawing_msgs/package.xml

@@ -0,0 +1,22 @@
+<?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>rj_drawing_msgs</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="[email protected]">Kyle Stachowicz</maintainer>
+    <maintainer email="[email protected]">Oswin So</maintainer>
+    <license>TODO: License declaration</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+    <buildtool_depend>rosidl_default_generators</buildtool_depend>
+
+    <depend>std_msgs</depend>
+    <depend>rj_geometry_msgs</depend>
+
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+</package>

rj_msgs/CMakeLists.txt

@@ -12,12 +12,11 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(rj_geometry_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
-find_package(rj_geometry_msgs REQUIRED)
 
 # ======================================================================
 # Define Targets
 # ======================================================================
-# This creates the target rj_geometry_msgs__rosidl_typesupport_cpp
+# This creates the target rj_msgs__rosidl_typesupport_cpp
 rosidl_generate_interfaces(
   rj_msgs
   # Messages

soccer/CMakeLists.txt

@@ -10,6 +10,7 @@ project(soccer LANGUAGES CXX)
 find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(rj_msgs REQUIRED)
+find_package(rj_drawing_msgs REQUIRED)
 find_package(rj_geometry_msgs REQUIRED)
 
 # Qt5
@@ -210,6 +211,7 @@ target_include_directories(robocup PUBLIC ${ROBOCUP_DEPS_INCLUDE_DIRS})
 target_link_libraries(robocup PUBLIC ${ROBOCUP_DEPS_SYSTEM_LIBRARIES} ${ROBOCUP_DEPS_LIBRARIES})
 
 ament_target_dependencies(robocup PUBLIC rj_geometry_msgs)
+ament_target_dependencies(robocup PUBLIC rj_drawing_msgs)
 ament_target_dependencies(robocup PUBLIC rj_msgs)
 
 # ---- rj_vision_filter ----

soccer/src/soccer/CMakeLists.txt

@@ -74,6 +74,7 @@ set(ROBOCUP_LIB_SRC
     ros2_temp/soccer_config_client.cpp
     ros2_temp/raw_vision_packet_sub.cpp
     ros2_temp/referee_sub.cpp
+    ros2_temp/debug_draw_interface.cpp
     window_evaluator.cpp)
 
 set(SOCCER_TEST_SRC

soccer/src/soccer/planning/planner/motion_command.hpp

@@ -147,6 +147,7 @@ struct RosConverter<Planning::WorldVelCommand, rj_msgs::msg::WorldVelMotionComma
             convert_to_ros(from.world_vel));
     }
 
+
     static Planning::WorldVelCommand from_ros(const rj_msgs::msg::WorldVelMotionCommand& from) {
         return Planning::WorldVelCommand{convert_from_ros(from.world_vel)};
     }

soccer/src/soccer/processor.cpp

@@ -68,7 +68,6 @@ Processor::Processor(bool sim, bool blue_team, const std::string& read_log_file)
 
     ros_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
 
-    referee_sub_ = std::make_unique<ros2_temp::RefereeSub>(&context_, ros_executor_.get());
     gameplay_module_ = std::make_shared<Gameplay::GameplayModule>(&context_);
     motion_control_ = std::make_unique<MotionControlNode>(&context_);
     planner_node_ = std::make_unique<Planning::PlannerNode>(&context_);
@@ -79,6 +78,9 @@ Processor::Processor(bool sim, bool blue_team, const std::string& read_log_file)
     // ROS2 temp nodes
     config_client_ = std::make_unique<ros2_temp::SoccerConfigClient>(&context_);
     raw_vision_packet_sub_ = std::make_unique<ros2_temp::RawVisionPacketSub>(&context_);
+    referee_sub_ = std::make_unique<ros2_temp::RefereeSub>(&context_, ros_executor_.get());
+    debug_draw_sub_ =
+        std::make_unique<ros2_temp::DebugDrawInterface>(&context_, ros_executor_.get());
     world_state_queue_ = std::make_unique<AsyncWorldStateMsgQueue>(
         "world_state_queue", vision_filter::topics::kWorldStatePub);
 
@@ -143,7 +145,9 @@ void Processor::run() {
         ////////////////
         // Inputs
         // TODO(#1558): Backport spin_all and use it for our main executor.
-        ros_executor_->spin_some();
+        for (int i = 0; i < 10; i++) {
+            ros_executor_->spin_some();
+        }
         sdl_joystick_node_->run();
         manual_control_node_->run();
 
@@ -210,6 +214,8 @@ void Processor::run() {
         // Processor Initialization Completed
         initialized_ = true;
 
+        debug_draw_sub_->run();
+
         {
             loop_mutex()->lock();
             // Log this entire frame

soccer/src/soccer/processor.hpp

@@ -4,33 +4,36 @@
 
 #pragma once
 
-#include <rj_protos/LogFrame.pb.h>
-#include <rj_topic_utils/async_message_queue.hpp>
-#include <ros2_temp/raw_vision_packet_sub.hpp>
-#include <ros2_temp/soccer_config_client.hpp>
-
-#include <rj_geometry/point.hpp>
-#include <rj_geometry/pose.hpp>
-#include <rj_geometry/transform_matrix.hpp>
-#include <logger.hpp>
-#include <system_state.hpp>
 #include <mutex>
 #include <optional>
+#include <vector>
+
 #include <rclcpp/executors/single_threaded_executor.hpp>
+
+#include <logger.hpp>
 #include <referee/external_referee.hpp>
+#include <rj_geometry/point.hpp>
+#include <rj_geometry/pose.hpp>
+#include <rj_geometry/transform_matrix.hpp>
 #include <rj_msgs/msg/world_state.hpp>
+#include <rj_protos/LogFrame.pb.h>
+#include <rj_topic_utils/async_message_queue.hpp>
+#include <ros2_temp/debug_draw_interface.hpp>
+#include <ros2_temp/raw_vision_packet_sub.hpp>
 #include <ros2_temp/referee_sub.hpp>
-#include <vector>
+#include <ros2_temp/soccer_config_client.hpp>
+#include <system_state.hpp>
 
 #include "context.hpp"
 #include "gr_sim_communicator.hpp"
-#include "node.hpp"
 #include "joystick/manual_control_node.hpp"
 #include "joystick/sdl_joystick_node.hpp"
 #include "motion/motion_control_node.hpp"
+#include "node.hpp"
 #include "planning/planner_node.hpp"
 #include "radio/radio.hpp"
 #include "radio/radio_node.hpp"
+
 #include "rc-fshare/rtp.hpp"
 
 class Configuration;
@@ -187,6 +190,7 @@ class Processor {
     std::unique_ptr<ros2_temp::SoccerConfigClient> config_client_;
     std::unique_ptr<ros2_temp::RawVisionPacketSub> raw_vision_packet_sub_;
     std::unique_ptr<ros2_temp::RefereeSub> referee_sub_;
+    std::unique_ptr<ros2_temp::DebugDrawInterface> debug_draw_sub_;
 
     std::vector<Node*> nodes_;
 

soccer/src/soccer/ros2_temp/debug_draw_interface.cpp

@@ -0,0 +1,58 @@
+#include "debug_draw_interface.hpp"
+
+#include <rj_constants/topic_names.hpp>
+
+namespace ros2_temp {
+
+constexpr size_t kDebugDrawQueueSize = 10;
+
+DebugDrawInterface::DebugDrawInterface(Context* context, rclcpp::Executor* executor)
+    : context_(context) {
+    node_ = std::make_shared<rclcpp::Node>("_debug_draw_interface");
+    debug_draw_sub_ = node_->create_subscription<rj_drawing_msgs::msg::DebugDraw>(
+        viz::topics::kDebugDrawPub, rclcpp::QoS(kDebugDrawQueueSize),
+        [this](rj_drawing_msgs::msg::DebugDraw::SharedPtr debug_draw) {  // NOLINT
+            latest_[debug_draw->layer] = debug_draw;
+        });
+
+    executor->add_node(node_);
+}
+
+void DebugDrawInterface::run() {
+    const auto& color_to_qt = [](const rj_drawing_msgs::msg::DrawColor& color) {
+        return QColor::fromRgb(color.r, color.g, color.b, color.a);
+    };
+
+    for (const auto& [layer, debug_draw] : latest_) {
+        for (const auto& shapes : debug_draw->shapes) {
+            context_->debug_drawer.draw_shape_set(rj_convert::convert_from_ros(shapes.shapes),
+                                                  color_to_qt(shapes.color),
+                                                  QString::fromStdString(layer));
+        }
+        for (const auto& segment : debug_draw->segments) {
+            context_->debug_drawer.draw_segment(rj_convert::convert_from_ros(segment.segment),
+                                                color_to_qt(segment.color),
+                                                QString::fromStdString(layer));
+        }
+        for (const auto& pose : debug_draw->poses) {
+            // TODO(#1584): Handle poses
+        }
+        for (const auto& path : debug_draw->paths) {
+            auto* debug_path = context_->debug_drawer.add_debug_path();
+            for (const auto& point : path.points) {
+                auto* new_point = debug_path->add_points();
+                new_point->mutable_pos()->set_x(point.x);
+                new_point->mutable_pos()->set_y(point.y);
+
+                // TODO(#1584): Use color in trajectory
+            }
+        }
+        for (const auto& text : debug_draw->debug_text) {
+            context_->debug_drawer.draw_text(
+                QString::fromStdString(text.text), rj_convert::convert_from_ros(text.position),
+                color_to_qt(text.color), QString::fromStdString(layer));
+        }
+    }
+}
+
+}  // namespace ros2_temp

soccer/src/soccer/ros2_temp/debug_draw_interface.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <rj_drawing_msgs/msg/debug_draw.hpp>
+
+#include "context.hpp"
+
+namespace ros2_temp {
+
+class DebugDrawInterface : public Node {
+public:
+    DebugDrawInterface(Context* context, rclcpp::Executor* executor);
+
+    void run() override;
+
+private:
+    rclcpp::Node::SharedPtr node_;
+    Context* context_;
+    rclcpp::Subscription<rj_drawing_msgs::msg::DebugDraw>::SharedPtr debug_draw_sub_;
+    std::unordered_map<std::string, rj_drawing_msgs::msg::DebugDraw::SharedPtr> latest_;
+};
+
+}  // namespace ros2_temp