diff --git a/CMakeLists.txt b/CMakeLists.txt
index 86c0c7f189f..83db735312c 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/rj_constants/include/rj_constants/topic_names.hpp b/rj_constants/include/rj_constants/topic_names.hpp
index 7be87f7f834..58dcf1541e6 100644
--- a/rj_constants/include/rj_constants/topic_names.hpp
+++ b/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";
diff --git a/rj_drawing_msgs/CMakeLists.txt b/rj_drawing_msgs/CMakeLists.txt
new file mode 100644
index 00000000000..578eadfabcd
--- /dev/null
+++ b/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()
diff --git a/rj_drawing_msgs/msg/DebugDraw.msg b/rj_drawing_msgs/msg/DebugDraw.msg
new file mode 100644
index 00000000000..57b1834b5db
--- /dev/null
+++ b/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
diff --git a/rj_drawing_msgs/msg/DrawColor.msg b/rj_drawing_msgs/msg/DrawColor.msg
new file mode 100644
index 00000000000..ed163a9ba87
--- /dev/null
+++ b/rj_drawing_msgs/msg/DrawColor.msg
@@ -0,0 +1,4 @@
+int8 r
+int8 g
+int8 b
+int8 a
diff --git a/rj_drawing_msgs/msg/DrawPath.msg b/rj_drawing_msgs/msg/DrawPath.msg
new file mode 100644
index 00000000000..16c857e8226
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/rj_drawing_msgs/msg/DrawPose.msg b/rj_drawing_msgs/msg/DrawPose.msg
new file mode 100644
index 00000000000..55ca711b9f4
--- /dev/null
+++ b/rj_drawing_msgs/msg/DrawPose.msg
@@ -0,0 +1,2 @@
+rj_geometry_msgs/Pose pose
+DrawColor color
diff --git a/rj_drawing_msgs/msg/DrawSegment.msg b/rj_drawing_msgs/msg/DrawSegment.msg
new file mode 100644
index 00000000000..9cc162db097
--- /dev/null
+++ b/rj_drawing_msgs/msg/DrawSegment.msg
@@ -0,0 +1,2 @@
+rj_geometry_msgs/Segment segment
+DrawColor color
\ No newline at end of file
diff --git a/rj_drawing_msgs/msg/DrawShapes.msg b/rj_drawing_msgs/msg/DrawShapes.msg
new file mode 100644
index 00000000000..97f3dbe786d
--- /dev/null
+++ b/rj_drawing_msgs/msg/DrawShapes.msg
@@ -0,0 +1,2 @@
+rj_geometry_msgs/ShapeSet shapes
+DrawColor color
\ No newline at end of file
diff --git a/rj_drawing_msgs/msg/DrawText.msg b/rj_drawing_msgs/msg/DrawText.msg
new file mode 100644
index 00000000000..419926be4dd
--- /dev/null
+++ b/rj_drawing_msgs/msg/DrawText.msg
@@ -0,0 +1,3 @@
+string text
+rj_geometry_msgs/Point position
+DrawColor color
\ No newline at end of file
diff --git a/rj_drawing_msgs/package.xml b/rj_drawing_msgs/package.xml
new file mode 100644
index 00000000000..567547f118c
--- /dev/null
+++ b/rj_drawing_msgs/package.xml
@@ -0,0 +1,22 @@
+
+
+
+ rj_drawing_msgs
+ 0.0.0
+ TODO: Package description
+ Kyle Stachowicz
+ Oswin So
+ TODO: License declaration
+
+ ament_cmake
+ rosidl_default_generators
+
+ std_msgs
+ rj_geometry_msgs
+
+ rosidl_interface_packages
+
+
+ ament_cmake
+
+
diff --git a/rj_msgs/CMakeLists.txt b/rj_msgs/CMakeLists.txt
index 912c455ac0c..14dd0bf165f 100644
--- a/rj_msgs/CMakeLists.txt
+++ b/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
diff --git a/soccer/CMakeLists.txt b/soccer/CMakeLists.txt
index 2c95dfc6b44..5e68a7b2e46 100644
--- a/soccer/CMakeLists.txt
+++ b/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 ----
diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt
index 146c064df11..6d24793a1f5 100644
--- a/soccer/src/soccer/CMakeLists.txt
+++ b/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
diff --git a/soccer/src/soccer/planning/planner/motion_command.hpp b/soccer/src/soccer/planning/planner/motion_command.hpp
index 9692d0447c3..5070bed3c26 100644
--- a/soccer/src/soccer/planning/planner/motion_command.hpp
+++ b/soccer/src/soccer/planning/planner/motion_command.hpp
@@ -147,6 +147,7 @@ struct RosConverter();
- referee_sub_ = std::make_unique(&context_, ros_executor_.get());
gameplay_module_ = std::make_shared(&context_);
motion_control_ = std::make_unique(&context_);
planner_node_ = std::make_unique(&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(&context_);
raw_vision_packet_sub_ = std::make_unique(&context_);
+ referee_sub_ = std::make_unique(&context_, ros_executor_.get());
+ debug_draw_sub_ =
+ std::make_unique(&context_, ros_executor_.get());
world_state_queue_ = std::make_unique(
"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
diff --git a/soccer/src/soccer/processor.hpp b/soccer/src/soccer/processor.hpp
index a275a090e3f..95800635893 100644
--- a/soccer/src/soccer/processor.hpp
+++ b/soccer/src/soccer/processor.hpp
@@ -4,33 +4,36 @@
#pragma once
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
#include
#include
+#include
+
#include
+
+#include
#include
+#include
+#include
+#include
#include
+#include
+#include
+#include
+#include
#include
-#include
+#include
+#include
#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 config_client_;
std::unique_ptr raw_vision_packet_sub_;
std::unique_ptr referee_sub_;
+ std::unique_ptr debug_draw_sub_;
std::vector nodes_;
diff --git a/soccer/src/soccer/ros2_temp/debug_draw_interface.cpp b/soccer/src/soccer/ros2_temp/debug_draw_interface.cpp
new file mode 100644
index 00000000000..403f90a23b0
--- /dev/null
+++ b/soccer/src/soccer/ros2_temp/debug_draw_interface.cpp
@@ -0,0 +1,58 @@
+#include "debug_draw_interface.hpp"
+
+#include
+
+namespace ros2_temp {
+
+constexpr size_t kDebugDrawQueueSize = 10;
+
+DebugDrawInterface::DebugDrawInterface(Context* context, rclcpp::Executor* executor)
+ : context_(context) {
+ node_ = std::make_shared("_debug_draw_interface");
+ debug_draw_sub_ = node_->create_subscription(
+ 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
\ No newline at end of file
diff --git a/soccer/src/soccer/ros2_temp/debug_draw_interface.hpp b/soccer/src/soccer/ros2_temp/debug_draw_interface.hpp
new file mode 100644
index 00000000000..2527498b0e3
--- /dev/null
+++ b/soccer/src/soccer/ros2_temp/debug_draw_interface.hpp
@@ -0,0 +1,24 @@
+#pragma once
+
+#include
+
+#include
+
+#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::SharedPtr debug_draw_sub_;
+ std::unordered_map latest_;
+};
+
+} // namespace ros2_temp