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